Merge branch 'master' into beta

.gitmodules

@@ -19,3 +19,6 @@
 [submodule "src/lib/eigen"]
 	path = src/lib/eigen
 	url = https://github.com/PX4/eigen.git
+[submodule "src/lib/dspal"]
+	path = src/lib/dspal
+	url = https://github.com/mcharleb/dspal.git

.travis.yml

@@ -56,6 +56,9 @@ before_script:
   - ln -s /usr/bin/ccache ~/bin/g++-4.8
   - ln -s /usr/bin/ccache ~/bin/gcc-4.8
   - export PATH=~/bin:$PATH
+# grab astyle 2.05.1
+  - wget -O ~/bin/astyle https://github.com/PX4/astyle/releases/download/2.05.1/astyle-linux && chmod +x ~/bin/astyle
+  - astyle --version
 
 env:
   global:
@@ -66,6 +69,7 @@ env:
     - PX4_AWS_BUCKET=px4-travis
 
 script:
+  - make check_format
   - ccache -z
   - arm-none-eabi-gcc --version
   - echo 'Building POSIX Firmware..' && echo -en 'travis_fold:start:script.1\\r'

Makefile

@@ -222,9 +222,9 @@ qurtrun:
 tests:	generateuorbtopicheaders
 	$(Q) (mkdir -p $(PX4_BASE)/unittests/build && cd $(PX4_BASE)/unittests/build && cmake .. && $(MAKE) --no-print-directory unittests)
 
-.PHONY: format check_format
+.PHONY: check_format
 check_format:
-	$(Q) (./Tools/check_code_style.sh | sort -n)
+	$(Q) (./Tools/check_code_style.sh)
 
 #
 # Cleanup targets.  'clean' should remove all built products and force

README.md

@@ -16,7 +16,7 @@ This repository contains the PX4 Flight Core, with the main applications located
   * [Downloads](http://px4.io/firmware/downloads)
 *   Releases
   * [Downloads](https://github.com/PX4/Firmware/releases)
-*   Mailing list: [Google Groups](http://groups.google.com/group/px4users)
+*   Forum / Mailing list: [Google Groups](http://groups.google.com/group/px4users)
 
 ### Users ###
 
@@ -29,7 +29,7 @@ Contributing guide:
   * [PX4 Contribution Guide](http://px4.io/dev/contributing)
 
 Software in the Loop guide:
-Use software in the loop [to get started with the codebase](https://github.com/PX4/Firmware/tree/master/posix-configs/SITL)
+Use software in the loop [to get started with the codebase](https://pixhawk.org/dev/simulation/native_sitl).
 
 Developer guide:
 http://px4.io/dev/
@@ -38,8 +38,10 @@ Testing guide:
 http://px4.io/dev/unit_tests
 
 This repository contains code supporting these boards:
+  * [Snapdragon Flight](https://www.intrinsyc.com/qualcomm-snapdragon-flight/)
   * FMUv1.x
-  * FMUv2.x
+  * FMUv2.x (Pixhawk)
+  * FMUv3.x (Pixhawk 2)
   * AeroCore (v1 and v2)
   * STM32F4Discovery (basic support) [Tutorial](https://pixhawk.org/modules/stm32f4discovery)
 

ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil

@@ -39,8 +39,5 @@ then
 	param set FW_RR_P 0.3
 fi
 
-# Enable gamepad / joystick support
-param set COM_RC_IN_MODE 2
-
 set HIL yes
 set MIXER AERT

ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil

@@ -11,7 +11,4 @@ sh /etc/init.d/rc.mc_defaults
 
 set MIXER quad_x
 
-# Enable gamepad / joystick support
-param set COM_RC_IN_MODE 2
-
 set HIL yes

ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil

@@ -11,7 +11,4 @@ sh /etc/init.d/rc.mc_defaults
 
 set MIXER quad_+
 
-# Enable gamepad / joystick support
-param set COM_RC_IN_MODE 2
-
 set HIL yes

ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil

@@ -11,7 +11,4 @@ sh /etc/init.d/rc.fw_defaults
 
 set HIL yes
 
-# Enable gamepad / joystick support
-param set COM_RC_IN_MODE 2
-
 set MIXER AERT

ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil

@@ -36,8 +36,5 @@ fi
 
 set HIL yes
 
-# Enable gamepad / joystick support
-param set COM_RC_IN_MODE 2
-
 # Set the AERT mixer for HIL (even if the malolo is a flying wing)
 set MIXER AERT

ROMFS/px4fmu_common/init.d/2101_fw_AERT

@@ -20,3 +20,7 @@
 sh /etc/init.d/rc.fw_defaults
 
 set MIXER AERT
+
+# The ESC requires a specific pulse to arm.
+set PWM_OUT 4
+set PWM_DISARMED p:PWM_DISARMED

ROMFS/px4fmu_common/init.d/2104_fw_AETR

@@ -20,3 +20,7 @@
 sh /etc/init.d/rc.fw_defaults
 
 set MIXER AETR
+
+# The ESC requires a specific pulse to arm.
+set PWM_OUT 3
+set PWM_DISARMED p:PWM_DISARMED

ROMFS/px4fmu_common/init.d/rc.sensors

@@ -110,8 +110,6 @@ else
 	fi
 fi
 
-#
-# Start sensors
-#
+# Wait 20 ms for sensors (because we need to wait for the HRT and work queue callbacks to fire)
+usleep 20000
 sensors start
-

ROMFS/px4fmu_common/init.d/rc.uavcan

@@ -3,26 +3,10 @@
 # UAVCAN initialization script.
 #
 
-#
-# Mirriring the UAVCAN_ENABLE param value to an eponymous environment variable.
-# TODO there should be a smarter way.
-#
-set UAVCAN_ENABLE 0
-if param compare UAVCAN_ENABLE 1
-then
-	set UAVCAN_ENABLE 1
-fi
-if param compare UAVCAN_ENABLE 2
-then
-	set UAVCAN_ENABLE 2
-fi
-
-echo "[i] UAVCAN_ENABLE is $UAVCAN_ENABLE"
-
 #
 # Starting stuff according to UAVCAN_ENABLE value
 #
-if [ $UAVCAN_ENABLE -ge 1 ]
+if param greater UAVCAN_ENABLE 0
 then
 	if uavcan start
 	then
@@ -33,7 +17,7 @@ then
 	fi
 fi
 
-if [ $UAVCAN_ENABLE -ge 2 ]
+if param greater UAVCAN_ENABLE 1
 then
 	if uavcan start fw
 	then
@@ -43,10 +27,3 @@ then
 		tone_alarm $TUNE_ERR
 	fi
 fi
-
-if [ $UAVCAN_ENABLE -ge 1 ]
-then
-	# First sensor publisher to initialize takes lowest instance ID
-	# This delay ensures that UAVCAN-interfaced sensors will be allocated on lowest instance IDs
-	sleep 8
-fi

ROMFS/px4fmu_common/init.d/rcS

@@ -365,11 +365,6 @@ then
 	then
 	fi
 
-	#
-	# UAVCAN
-	#
-	sh /etc/init.d/rc.uavcan
-
 	#
 	# Sensors System (start before Commander so Preflight checks are properly run)
 	#
@@ -389,8 +384,6 @@ then
 	unset GPS_FAKE
 
 	# Needs to be this early for in-air-restarts
-	# Wait 10 ms for sensors (workaround for airspeed, to be removed)
-	usleep 10000
 	commander start
 
 	#
@@ -578,6 +571,11 @@ then
 		fi
 	fi
 
+	#
+	# UAVCAN
+	#
+	sh /etc/init.d/rc.uavcan
+
 	#
 	# Logging
 	#

Tools/check_code_style.sh

@@ -1,16 +1,45 @@
 #!/usr/bin/env bash
 set -eu
 failed=0
-for fn in $(find . -path './src/lib/uavcan' -prune -o \
-                   -path './src/lib/mathlib/CMSIS' -prune -o \
-                   -path './src/lib/eigen' -prune -o \
-                   -path './src/modules/attitude_estimator_ekf/codegen' -prune -o \
-                   -path './NuttX' -prune -o \
+for fn in $(find src/examples \
+                 src/systemcmds \
+                 src/include \
+                 src/drivers/blinkm \
+                 src/drivers/bma180 \
+                 src/drivers/pca9685 \
+                 src/drivers/pca8574 \
+                 src/drivers/md25 \
+                 src/drivers/ms5611 \
+                 src/drivers/stm32 \
+                 src/drivers/px4io \
+                 src/drivers/px4fmu \
+                 src/lib/launchdetection \
+                 src/modules/bottle_drop \
+                 src/modules/dataman \
+                 src/modules/fixedwing_backside \
+                 src/modules/segway \
+                 src/modules/unit_test \
+                 src/modules/systemlib \
                    -path './Build' -prune -o \
                    -path './mavlink' -prune -o \
-                   -path './unittests/gtest' -prune -o \
+                   -path './NuttX' -prune -o \
+                   -path './src/lib/eigen' -prune -o \
+                   -path './src/lib/mathlib/CMSIS' -prune -o \
+                   -path './src/lib/uavcan' -prune -o \
+                   -path './src/modules/attitude_estimator_ekf/codegen' -prune -o \
+                   -path './src/modules/ekf_att_pos_estimator' -prune -o \
+                   -path './src/modules/sdlog2' -prune -o \
+                   -path './src/modules/uORB' -prune -o \
+                   -path './src/modules/vtol_att_control' -prune -o \
                    -path './unittests/build' -prune -o \
-                   -name '*.c' -o -name '*.cpp' -o -name '*.hpp' -o -name '*.h' -type f); do
+                   -path './unittests/gtest' -prune -o \
+                   -name '*.c' -o -name '*.cpp' -o -name '*.hpp' -o -name '*.h' \
+                   -not -name '*generated*' \
+                   -not -name '*uthash.h' \
+                   -not -name '*utstring.h' \
+                   -not -name '*utlist.h' \
+                   -not -name '*utarray.h' \
+                   -type f); do
   if [ -f "$fn" ];
   then
     ./Tools/fix_code_style.sh --quiet < $fn > $fn.pretty
@@ -18,7 +47,7 @@ for fn in $(find . -path './src/lib/uavcan' -prune -o \
     rm -f $fn.pretty
     if [ $diffsize -ne 0 ]; then
       failed=1
-      echo $diffsize $fn
+      echo $fn 'bad formatting, please run "./Tools/fix_code_style.sh' $fn'"'
     fi
   fi
 done
@@ -27,6 +56,6 @@ if [ $failed -eq 0 ]; then
     echo "Format checks passed"
     exit 0
 else
-    echo "Format checks failed; please run ./Tools/fix_code_style.sh on each file"
+    echo "Format checks failed"
     exit 1
 fi

Tools/fix_code_style.sh

@@ -1,4 +1,14 @@
-#!/bin/sh
+#!/bin/bash
+
+ASTYLE_VER=`astyle --version`
+ASTYLE_VER_REQUIRED="Artistic Style Version 2.05.1"
+
+if [ "$ASTYLE_VER" != "$ASTYLE_VER_REQUIRED" ]; then
+  echo "Error: you're using ${ASTYLE_VER}, but PX4 requires ${ASTYLE_VER_REQUIRED}"
+  echo "You can get the correct version here: https://github.com/PX4/astyle/releases/tag/2.05.1"
+  exit 1
+fi
+
 DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )
 astyle \
     --options=$DIR/astylerc          \

Tools/px4params/srcparser.py

@@ -200,7 +200,7 @@ class SourceParser(object):
                     m = self.re_px4_parameter_definition.match(line)
                     if m:
                         tp, name = m.group(1, 2)
-                        if default_var.has_key(name+'_DEFAULT'):
+                        if (name+'_DEFAULT') in default_var:
                             defval = default_var[name+'_DEFAULT']
                 if tp is not None:
                     # Remove trailing type specifier from numbers: 0.1f => 0.1

Tools/px_romfs_pruner.py

@@ -76,7 +76,7 @@ def main():
                                                         pruned_content += line
 
                         # overwrite old scratch file
-                        with open(file_path, "w") as f:
+                        with open(file_path, "wb") as f:
                                 f.write(pruned_content)
 
 

Vagrantfile

@@ -73,12 +73,12 @@ Vagrant.configure(2) do |config|
   # Enable provisioning with a shell script. Additional provisioners such as
   # Puppet, Chef, Ansible, Salt, and Docker are also available. Please see the
   # documentation for more information about their specific syntax and use.
-  config.vm.provision "shell", inline: <<-SHELL
+  config.vm.provision "shell", privileged: false, inline: <<-SHELL
     # Ensure we start in the Firmware folder
     echo "cd /Firmware" >> ~/.bashrc
     # Install software
     sudo apt-get update
-    sudo apt-get install -y build-essential ccache cmake clang-3.5 lldb-3.5 g++-4.8 gcc-4.8 genromfs libc6-i386 libncurses5-dev python-argparse python-empy python-serial s3cmd texinfo zlib1g-dev git-core
+    sudo apt-get install -y build-essential ccache cmake clang-3.5 lldb-3.5 g++-4.8 gcc-4.8 genromfs libc6-i386 libncurses5-dev python-argparse python-empy python-serial s3cmd texinfo zlib1g-dev git-core zip
     pushd .
     cd ~
     wget -q https://launchpadlibrarian.net/186124160/gcc-arm-none-eabi-4_8-2014q3-20140805-linux.tar.bz2

launch/ardrone.launch

@@ -1,8 +1,10 @@
 <launch>
 <arg name="ns"/>
+<arg name="mavlink_fcu_url" default="udp://localhost:14565@localhost:14560"/>
 
 <include file="$(find px4)/launch/multicopter_x.launch">
 	<arg name="ns" value="$(arg ns)"/>
+	<arg name="mavlink_fcu_url" value="$(arg mavlink_fcu_url)" />
 </include>
 
 <group ns="$(arg ns)">

launch/iris.launch

@@ -1,8 +1,10 @@
 <launch>
 <arg name="ns"/>
+<arg name="mavlink_fcu_url" default="udp://localhost:14565@localhost:14560"/>
 
 <include file="$(find px4)/launch/multicopter_w.launch">
 	<arg name="ns" value="$(arg ns)"/>
+	<arg name="mavlink_fcu_url" value="$(arg mavlink_fcu_url)" />
 </include>
 
 <group ns="$(arg ns)">

launch/multi_uav.launch

@@ -0,0 +1,55 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="headless" default="false"/>
+  <arg name="gui" default="true"/>
+  <arg name="enable_logging" default="false"/>
+  <arg name="enable_ground_truth" default="true"/>
+  <arg name="debug" default="false"/>
+  <arg name="ns" default="iris"/>
+  <arg name="log_file" default="$(arg ns)"/>
+
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="$(find rotors_gazebo)/worlds/basic.world"/>
+    <arg name="debug" value="$(arg debug)" />
+    <arg name="headless" value="$(arg headless)"/>
+    <arg name="gui" value="$(arg gui)"/>
+  </include>
+  
+   <group ns="iris">
+      <include file="$(find rotors_gazebo)/launch/spawn_iris.launch">
+        <arg name="model" value="$(find rotors_description)/urdf/iris_gesture_sensors.gazebo" />
+        <arg name="enable_logging" value="$(arg enable_logging)" />
+        <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
+        <arg name="log_file" value="$(arg log_file)"/>
+        <arg name="x" value="1"/>
+        <arg name="y" value="0"/>
+      </include>
+      <arg name="fcu_url" default="udp://localhost:14560@localhost:14565" />
+      <include file="$(find mavros)/launch/px4.launch">
+        <arg name="fcu_url" value="$(arg fcu_url)" />
+      </include>
+   </group>
+   <include file="$(find px4)/launch/iris.launch">
+     <arg name="ns" value="iris"/>
+   </include>   
+      
+   <group ns="ardrone">
+      <include file="$(find rotors_gazebo)/launch/spawn_ardrone.launch">
+        <arg name="model" value="$(find rotors_description)/urdf/ardrone_base.xacro" />
+        <arg name="enable_logging" value="$(arg enable_logging)" />
+        <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
+        <arg name="log_file" value="$(arg log_file)"/>
+        <arg name="x" value="-1"/>
+        <arg name="y" value="0"/>
+      </include>
+      <arg name="fcu_url" default="udp://localhost:14570@localhost:14575" />
+      <include file="$(find mavros)/launch/px4.launch">
+        <arg name="fcu_url" value="$(arg fcu_url)" />
+      </include>
+   </group>
+   <include file="$(find px4)/launch/ardrone.launch">
+	<arg name="ns" value="ardrone"/>
+	<arg name="mavlink_fcu_url" value="udp://localhost:14575@localhost:14570"/>
+   </include>
+
+</launch>   

launch/multicopter.launch

@@ -1,5 +1,6 @@
 <launch>
 <arg name="ns"/>
+<arg name="mavlink_fcu_url"/>
 
 <group ns="$(arg ns)">
 	<node pkg="joy" name="joy_node" type="joy_node"/>
@@ -10,7 +11,9 @@
 	<node pkg="px4" name="position_estimator" type="position_estimator"/>
 	<node pkg="px4" name="mc_att_control" type="mc_att_control"/>
 	<node pkg="px4" name="mc_pos_control" type="mc_pos_control"/>
-	<node pkg="px4" name="mavlink" type="mavlink"/>
+	<node pkg="px4" name="mavlink" type="mavlink">  
+	  <param name="mavlink_fcu_url" value="$(arg mavlink_fcu_url)" type="str"/>
+	</node>
 	<!-- <node pkg="rosbag" type="record" name="record" output="screen" args="-a -O px4_multicopter"/> -->
 </group>
 

launch/multicopter_w.launch

@@ -1,8 +1,10 @@
 <launch>
 <arg name="ns"/>
+<arg name="mavlink_fcu_url"/>
 
 <include file="$(find px4)/launch/multicopter.launch">
 	<arg name="ns" value="$(arg ns)"/>
+	<arg name="mavlink_fcu_url" value="$(arg mavlink_fcu_url)" />
 </include>
 
 <group ns="$(arg ns)">

launch/multicopter_x.launch

@@ -1,8 +1,10 @@
 <launch>
 <arg name="ns"/>
+<arg name="mavlink_fcu_url"/>
 
 <include file="$(find px4)/launch/multicopter.launch">
 	<arg name="ns" value="$(arg ns)"/>
+	<arg name="mavlink_fcu_url" value="$(arg mavlink_fcu_url)" />
 </include>
 
 <group ns="$(arg ns)">

makefiles/nuttx/config_px4fmu-v2_default.mk

@@ -79,7 +79,7 @@ MODULES 	+= modules/land_detector
 #
 # Estimation modules (EKF/ SO3 / other filters)
 #
-# Too high RAM usage due to static allocations
+# Removed from build due to large static allocations
 #MODULES		+= modules/attitude_estimator_ekf
 MODULES		+= modules/attitude_estimator_q
 MODULES		+= modules/ekf_att_pos_estimator

makefiles/posix-arm/config_eagle_adsp.mk

@@ -0,0 +1,57 @@
+#
+# Makefile for the EAGLE *default* configuration
+#
+
+#
+# Board support modules
+#
+MODULES		+= drivers/device
+
+#
+# System commands
+#
+MODULES	+= systemcmds/param
+MODULES	+= systemcmds/ver
+
+#
+# General system control
+#
+MODULES		+= modules/mavlink
+
+#
+# Estimation modules (EKF/ SO3 / other filters)
+#
+
+#
+# Vehicle Control
+#
+
+#
+# Library modules
+#
+MODULES		+= modules/systemlib
+MODULES		+= modules/uORB
+MODULES		+= modules/dataman
+
+#
+# Libraries
+#
+MODULES		+= lib/mathlib
+MODULES		+= lib/mathlib/math/filter
+MODULES		+= lib/geo
+MODULES		+= lib/geo_lookup
+MODULES		+= lib/conversion
+#
+# Linux port
+#
+MODULES		+= platforms/posix/px4_layer
+MODULES		+= platforms/posix/work_queue
+
+#
+# Unit tests
+#
+
+#
+# muorb fastrpc changes.
+#
+MODULES		+= modules/muorb/krait

makefiles/posix-arm/config_eagle_hil.mk

@@ -6,6 +6,7 @@
 # Board support modules
 #
 MODULES		+= drivers/device
+MODULES		+= drivers/boards/sitl
 #MODULES		+= drivers/blinkm
 #MODULES		+= drivers/pwm_out_sim
 #MODULES		+= drivers/rgbled

makefiles/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk

@@ -39,14 +39,17 @@
 #
 CROSSDEV		 = arm-linux-gnueabihf-
 
-CC			 = $(CROSSDEV)gcc
-CXX			 = $(CROSSDEV)g++
-CPP			 = $(CROSSDEV)gcc -E
-LD			 = $(CROSSDEV)ld
-AR			 = $(CROSSDEV)ar rcs
-NM			 = $(CROSSDEV)nm
-OBJCOPY			 = $(CROSSDEV)objcopy
-OBJDUMP			 = $(CROSSDEV)objdump
+CC			 ?= $(CROSSDEV)gcc
+CXX			 ?= $(CROSSDEV)g++
+CPP			 ?= $(CROSSDEV)gcc -E
+LD			 ?= $(CROSSDEV)ld
+AR			 ?= $(CROSSDEV)ar rcs
+NM			 ?= $(CROSSDEV)nm
+OBJCOPY			 ?= $(CROSSDEV)objcopy
+OBJDUMP			 ?= $(CROSSDEV)objdump
+ifdef OECORE_NATIVE_SYSROOT
+AR := $(AR) rcs
+endif
 
 # Check if the right version of the toolchain is available
 #
@@ -57,7 +60,9 @@ ifeq (,$(findstring $(CROSSDEV_VER_FOUND), $(CROSSDEV_VER_SUPPORTED)))
 $(error Unsupported version of $(CC), found: $(CROSSDEV_VER_FOUND) instead of one in: $(CROSSDEV_VER_SUPPORTED))
 endif
 
-EXT_MUORB_LIB_ROOT = /opt/muorb_libs
+ifndef POSIX_EXT_LIB_ROOT
+$(error POSIX_EXT_LIB_ROOT is not set)
+endif
 
 # XXX this is pulled pretty directly from the fmu Make.defs - needs cleanup
 
@@ -71,37 +76,6 @@ ARCHCPUFLAGS_CORTEXA8    = -mtune=cortex-a8 \
                            -mfloat-abi=hard \
                            -mfpu=neon
  
-ARCHCPUFLAGS_CORTEXM4F	 = -mcpu=cortex-m4 \
-			   -mthumb \
-			   -march=armv7e-m \
-			   -mfpu=fpv4-sp-d16 \
-			   -mfloat-abi=hard
-
-ARCHCPUFLAGS_CORTEXM4	 = -mcpu=cortex-m4 \
-			   -mthumb \
-			   -march=armv7e-m \
-			   -mfloat-abi=soft
-
-ARCHCPUFLAGS_CORTEXM3	 = -mcpu=cortex-m3 \
-			   -mthumb \
-			   -march=armv7-m \
-			   -mfloat-abi=soft
-
-# Enabling stack checks if OS was build with them
-#
-TEST_FILE_STACKCHECK=$(WORK_DIR)nuttx-export/include/nuttx/config.h
-TEST_VALUE_STACKCHECK=CONFIG_ARMV7M_STACKCHECK\ 1
-ENABLE_STACK_CHECKS=$(shell $(GREP) -q "$(TEST_VALUE_STACKCHECK)" $(TEST_FILE_STACKCHECK); echo $$?;)
-ifeq ("$(ENABLE_STACK_CHECKS)","0")
-ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -finstrument-functions -ffixed-r10
-ARCHINSTRUMENTATIONDEFINES_CORTEXM4  = -finstrument-functions -ffixed-r10
-ARCHINSTRUMENTATIONDEFINES_CORTEXM3  =
-else
-ARCHINSTRUMENTATIONDEFINES_CORTEXM4F =
-ARCHINSTRUMENTATIONDEFINES_CORTEXM4  =
-ARCHINSTRUMENTATIONDEFINES_CORTEXM3  =
-endif
-
 # Pick the right set of flags for the architecture.
 #
 ARCHCPUFLAGS		 = $(ARCHCPUFLAGS_$(CONFIG_ARCH))
@@ -115,13 +89,13 @@ ifeq ($(CONFIG_BOARD),)
 $(error Board config does not define CONFIG_BOARD)
 endif
 ARCHDEFINES		+= -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) \
-                   -D__PX4_LINUX -D__PX4_POSIX \
-			       -Dnoreturn_function= \
-			       -I$(PX4_BASE)/src/modules/systemlib \
-			       -I$(PX4_BASE)/src/lib/eigen \
-			       -I$(PX4_BASE)/src/platforms/posix/include \
-			       -I$(PX4_BASE)/mavlink/include/mavlink \
-			       -Wno-error=shadow
+	                   -D__PX4_LINUX -D__PX4_POSIX \
+			   -Dnoreturn_function= \
+			   -I$(PX4_BASE)/src/modules/systemlib \
+			   -I$(PX4_BASE)/src/lib/eigen \
+			   -I$(PX4_BASE)/src/platforms/posix/include \
+			   -I$(PX4_BASE)/mavlink/include/mavlink \
+			   -Wno-error=shadow
 
 # optimisation flags
 #
@@ -189,7 +163,8 @@ LIBM			:= $(shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a)
 EXTRA_LIBS		+= -lpx4muorb -ladsprpc
 EXTRA_LIBS		+= -pthread -lm -lrt
 
-LIB_DIRS                += $(EXT_MUORB_LIB_ROOT)/krait/libs
+LIB_DIRS                += $(POSIX_EXT_LIB_ROOT)/libs
+INCLUDE_DIRS        += $(POSIX_EXT_LIB_ROOT)/inc
 
 # Flags we pass to the C compiler
 #

makefiles/posix/config_posix_sitl.mk

@@ -5,6 +5,7 @@
 #
 # Board support modules
 #
+MODULES		+= drivers/boards/sitl
 MODULES		+= drivers/device
 MODULES		+= drivers/blinkm
 MODULES		+= drivers/pwm_out_sim
@@ -96,7 +97,7 @@ MODULES 	+= platforms/posix/drivers/gpssim
 # Unit tests
 #
 #MODULES	+= platforms/posix/tests/hello
-#MODULES	+= platforms/posix/tests/vcdev_test
+MODULES	+= platforms/posix/tests/vcdev_test
 #MODULES	+= platforms/posix/tests/hrt_test
 #MODULES	+= platforms/posix/tests/wqueue
 

makefiles/qurt/config_qurt_adsp.mk

@@ -1,8 +1,19 @@
 #Added configuration specific flags here. 
 
+ifndef HEXAGON_DRIVERS_ROOT
+$(error HEXAGON_DRIVERS_ROOT is not set)
+endif
+ifndef EAGLE_DRIVERS_SRC
+$(error EAGLE_DRIVERS_SRC is not set)
+endif
+
+INCLUDE_DIRS += $(HEXAGON_DRIVERS_ROOT)/inc
+
 # For Actual flight we need to link against the driver dynamic libraries
-LDFLAGS	+= -L${DSPAL_ROOT}/mpu_spi/hexagon_Debug_dynamic_toolv64/ship -lmpu9x50
-LDFLAGS	+= -L${DSPAL_ROOT}/uart_esc/hexagon_Debug_dynamic_toolv64/ship -luart_esc
+LDFLAGS	+= -L${HEXAGON_DRIVERS_ROOT}/libs -lmpu9x50
+LDFLAGS	+= -luart_esc
+LDFLAGS	+= -lcsr_gps
+LDFLAGS	+= -lrc_receiver
 
 #
 # Makefile for the EAGLE QuRT *default* configuration
@@ -13,8 +24,10 @@ LDFLAGS	+= -L${DSPAL_ROOT}/uart_esc/hexagon_Debug_dynamic_toolv64/ship -luart_es
 #
 MODULES		+= drivers/device
 MODULES		+= modules/sensors
-#MODULES		+= platforms/qurt/drivers/mpu9x50
-#MODULES		+= platforms/qurt/drivers/uart_esc
+MODULES		+= $(EAGLE_DRIVERS_SRC)/mpu9x50
+MODULES		+= $(EAGLE_DRIVERS_SRC)/uart_esc
+MODULES		+= $(EAGLE_DRIVERS_SRC)/rc_receiver
+MODULES		+= $(EAGLE_DRIVERS_SRC)/csr_gps
 
 #
 # System commands
@@ -47,6 +60,7 @@ MODULES		+= modules/systemlib/mixer
 MODULES		+= modules/uORB
 #MODULES		+= modules/dataman
 MODULES		+= modules/commander
+MODULES 	+= modules/controllib
 
 #
 # Libraries
@@ -61,6 +75,7 @@ MODULES		+= lib/conversion
 # QuRT port
 #
 MODULES		+= platforms/qurt/px4_layer
+MODULES		+= platforms/posix/work_queue
 
 #
 # Unit tests

makefiles/qurt/config_qurt_hil.mk

@@ -6,6 +6,7 @@
 # Board support modules
 #
 MODULES		+= drivers/device
+MODULES		+= drivers/boards/sitl
 #MODULES		+= drivers/blinkm
 MODULES		+= drivers/pwm_out_sim
 MODULES		+= drivers/led

makefiles/qurt/firmware_qurt.mk

@@ -53,8 +53,7 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.a:	generateuorbtopicheaders
 		WORK_DIR=$(work_dir) \
 		$(FIRMWARE_GOAL)
 
-HEXAGON_TOOLS_ROOT	 = /opt/6.4.05
-#V_ARCH			 = v4
+HEXAGON_TOOLS_ROOT	 ?= /opt/6.4.03
 V_ARCH			 = v5
 HEXAGON_CLANG_BIN	 = $(addsuffix /qc/bin,$(HEXAGON_TOOLS_ROOT))
 SIM     = $(HEXAGON_CLANG_BIN)/hexagon-sim

makefiles/qurt/toolchain_hexagon.mk

@@ -35,14 +35,6 @@
 
 #$(info TOOLCHAIN  gnu-arm-eabi)
 
-#
-# Stop making if ADSP_LIB_ROOT is not set. This defines the path to
-# DspAL headers and driver headers
-#
-ifndef DSPAL_ROOT
-$(error DSPAL_ROOT is not set)
-endif
-
 # Toolchain commands. Normally only used inside this file.
 #
 HEXAGON_TOOLS_ROOT	 ?= /opt/6.4.03
@@ -57,7 +49,7 @@ HEXAGON_ISS_DIR		 = $(HEXAGON_TOOLS_ROOT)/qc/lib/iss
 TOOLSLIB		 = $(HEXAGON_TOOLS_ROOT)/dinkumware/lib/$(V_ARCH)/G0
 QCTOOLSLIB		 = $(HEXAGON_TOOLS_ROOT)/qc/lib/$(V_ARCH)/G0
 QURTLIB			 = $(HEXAGON_SDK_ROOT)/lib/common/qurt/ADSP$(V_ARCH)MP/lib
-#DSPAL			 = $(PX4_BASE)/../dspal_libs/libdspal.a
+DSPAL_INCS		 ?= $(PX4_BASE)/src/lib/dspal
 
 
 CC			 = $(HEXAGON_CLANG_BIN)/$(CROSSDEV)clang
@@ -93,8 +85,7 @@ DYNAMIC_LIBS            = \
 
 # Check if the right version of the toolchain is available
 #
-CROSSDEV_VER_SUPPORTED	 = 6.4.03
-#CROSSDEV_VER_SUPPORTED	 = 6.4.05
+CROSSDEV_VER_SUPPORTED	 = 6.4.03 6.4.05
 CROSSDEV_VER_FOUND	 = $(shell $(CC) --version | sed -n 's/^.*version \([\. 0-9]*\),.*$$/\1/p')
 
 ifeq (,$(findstring $(CROSSDEV_VER_FOUND), $(CROSSDEV_VER_SUPPORTED)))
@@ -122,18 +113,15 @@ ARCHDEFINES		+= -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) \
 			    -Dnoreturn_function= \
 			    -D__EXPORT= \
 			    -Drestrict= \
-                            -D_DEBUG \
-			    -I$(DSPAL_ROOT)/ \
-			    -I$(DSPAL_ROOT)/dspal/include \
-			    -I$(DSPAL_ROOT)/dspal/sys \
-			    -I$(DSPAL_ROOT)/dspal/sys/sys \
-			    -I$(DSPAL_ROOT)/mpu_spi/inc/ \
-			    -I$(DSPAL_ROOT)/uart_esc/inc/ \
+			    -D_DEBUG \
+			    -I$(DSPAL_INCS)/include \
+			    -I$(DSPAL_INCS)/sys \
 			    -I$(HEXAGON_TOOLS_ROOT)/gnu/hexagon/include \
 			    -I$(PX4_BASE)/src/lib/eigen \
 			    -I$(PX4_BASE)/src/platforms/qurt/include \
 			    -I$(PX4_BASE)/src/platforms/posix/include \
 			    -I$(PX4_BASE)/mavlink/include/mavlink \
+			    -I$(PX4_BASE)/../inc \
 			    -I$(QURTLIB)/..//include \
 			    -I$(HEXAGON_SDK_ROOT)/inc \
 			    -I$(HEXAGON_SDK_ROOT)/inc/stddef \

msg/input_rc.msg

@@ -5,6 +5,7 @@ uint8 RC_INPUT_SOURCE_PX4IO_SPEKTRUM = 3
 uint8 RC_INPUT_SOURCE_PX4IO_SBUS = 4
 uint8 RC_INPUT_SOURCE_PX4IO_ST24 = 5
 uint8 RC_INPUT_SOURCE_MAVLINK = 6
+uint8 RC_INPUT_SOURCE_QURT = 7
 
 uint8 RC_INPUT_MAX_CHANNELS = 18 	# Maximum number of R/C input channels in the system. S.Bus has up to 18 channels.
 

msg/sensor_accel.msg

@@ -1,8 +1,12 @@
 uint64 timestamp
+uint64 integral_dt	# integration time
 uint64 error_count
 float32 x		# acceleration in the NED X board axis in m/s^2
 float32 y		# acceleration in the NED Y board axis in m/s^2
 float32 z		# acceleration in the NED Z board axis in m/s^2
+float32 x_integral	# velocity in the NED X board axis in m/s over the integration time frame
+float32 y_integral	# velocity in the NED Y board axis in m/s over the integration time frame
+float32 z_integral	# velocity in the NED Z board axis in m/s over the integration time frame
 float32 temperature	# temperature in degrees celsius
 float32 range_m_s2	# range in m/s^2 (+- this value)
 float32 scaling

msg/sensor_combined.msg

@@ -26,96 +26,49 @@ uint32 SENSOR_PRIO_MAX = 255
 # NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only
 
 uint64 timestamp			# Timestamp in microseconds since boot, from gyro
-#
-int16[3] gyro_raw			# Raw sensor values of angular velocity
-float32[3] gyro_rad_s			# Angular velocity in radian per seconds
-uint32 gyro_priority			# Sensor priority
-uint32 gyro_errcount			# Error counter for gyro 0
-float32 gyro_temp			# Temperature of gyro 0
+uint64[3] gyro_timestamp			# Gyro timestamps
+int16[9] gyro_raw			# Raw sensor values of angular velocity
+float32[9] gyro_rad_s			# Angular velocity in radian per seconds
+uint32[3] gyro_priority			# Sensor priority
+float32[9] gyro_integral_rad		# delta angle in radians
+uint64[3] gyro_integral_dt			# delta time for gyro integral in us
+uint32[3] gyro_errcount			# Error counter for gyro 0
+float32[3] gyro_temp			# Temperature of gyro 0
 
-int16[3] accelerometer_raw		# Raw acceleration in NED body frame
-float32[3] accelerometer_m_s2		# Acceleration in NED body frame, in m/s^2
-int16 accelerometer_mode			# Accelerometer measurement mode
-float32 accelerometer_range_m_s2		# Accelerometer measurement range in m/s^2
-uint64 accelerometer_timestamp	# Accelerometer timestamp
-uint32 accelerometer_priority	# Sensor priority
-uint32 accelerometer_errcount	# Error counter for accel 0
-float32 accelerometer_temp		# Temperature of accel 0
+int16[9] accelerometer_raw		# Raw acceleration in NED body frame
+float32[9] accelerometer_m_s2		# Acceleration in NED body frame, in m/s^2
+float32[9] accelerometer_integral_m_s		# velocity in NED body frame, in m/s^2
+uint64[3] accelerometer_integral_dt		# delta time for accel integral in us
+int16[3] accelerometer_mode			# Accelerometer measurement mode
+float32[3] accelerometer_range_m_s2		# Accelerometer measurement range in m/s^2
+uint64[3] accelerometer_timestamp	# Accelerometer timestamp
+uint32[3] accelerometer_priority	# Sensor priority
+uint32[3] accelerometer_errcount	# Error counter for accel 0
+float32[3] accelerometer_temp		# Temperature of accel 0
 
-int16[3] magnetometer_raw		# Raw magnetic field in NED body frame
-float32[3] magnetometer_ga		# Magnetic field in NED body frame, in Gauss
-int16 magnetometer_mode			# Magnetometer measurement mode
-float32 magnetometer_range_ga		# measurement range in Gauss
-float32 magnetometer_cuttoff_freq_hz	# Internal analog low pass frequency of sensor
-uint64 magnetometer_timestamp	# Magnetometer timestamp
-uint32 magnetometer_priority			# Sensor priority
-uint32 magnetometer_errcount		# Error counter for mag 0
-float32 magnetometer_temp		# Temperature of mag 0
+int16[9] magnetometer_raw		# Raw magnetic field in NED body frame
+float32[9] magnetometer_ga		# Magnetic field in NED body frame, in Gauss
+int16[3] magnetometer_mode			# Magnetometer measurement mode
+float32[3] magnetometer_range_ga		# measurement range in Gauss
+float32[3] magnetometer_cuttoff_freq_hz	# Internal analog low pass frequency of sensor
+uint64[3] magnetometer_timestamp	# Magnetometer timestamp
+uint32[3] magnetometer_priority			# Sensor priority
+uint32[3] magnetometer_errcount		# Error counter for mag 0
+float32[3] magnetometer_temp		# Temperature of mag 0
 
-int16[3] gyro1_raw			# Raw sensor values of angular velocity
-float32[3] gyro1_rad_s			# Angular velocity in radian per seconds
-uint64 gyro1_timestamp		# Gyro timestamp
-uint32 gyro1_priority			# Sensor priority
-uint32 gyro1_errcount		# Error counter for gyro 1
-float32 gyro1_temp			# Temperature of gyro 1
-
-int16[3] accelerometer1_raw		# Raw acceleration in NED body frame
-float32[3] accelerometer1_m_s2		# Acceleration in NED body frame, in m/s^2
-uint64 accelerometer1_timestamp	# Accelerometer timestamp
-uint32 accelerometer1_priority	# Sensor priority
-uint32 accelerometer1_errcount	# Error counter for accel 1
-float32 accelerometer1_temp		# Temperature of accel 1
-
-int16[3] magnetometer1_raw		# Raw magnetic field in NED body frame
-float32[3] magnetometer1_ga		# Magnetic field in NED body frame, in Gauss
-uint64 magnetometer1_timestamp	# Magnetometer timestamp
-uint32 magnetometer1_priority			# Sensor priority
-uint32 magnetometer1_errcount	# Error counter for mag 1
-float32 magnetometer1_temp		# Temperature of mag 1
-
-int16[3] gyro2_raw			# Raw sensor values of angular velocity
-float32[3] gyro2_rad_s			# Angular velocity in radian per seconds
-uint64 gyro2_timestamp		# Gyro timestamp
-uint32 gyro2_priority			# Sensor priority
-uint32 gyro2_errcount		# Error counter for gyro 1
-float32 gyro2_temp			# Temperature of gyro 1
-
-int16[3] accelerometer2_raw		# Raw acceleration in NED body frame
-float32[3] accelerometer2_m_s2		# Acceleration in NED body frame, in m/s^2
-uint64 accelerometer2_timestamp	# Accelerometer timestamp
-uint32 accelerometer2_priority	# Sensor priority
-uint32 accelerometer2_errcount	# Error counter for accel 2
-float32 accelerometer2_temp		# Temperature of accel 2
-
-int16[3] magnetometer2_raw		# Raw magnetic field in NED body frame
-float32[3] magnetometer2_ga		# Magnetic field in NED body frame, in Gauss
-uint64 magnetometer2_timestamp	# Magnetometer timestamp
-uint32 magnetometer2_priority			# Sensor priority
-uint32 magnetometer2_errcount	# Error counter for mag 2
-float32 magnetometer2_temp		# Temperature of mag 2
-
-float32 baro_pres_mbar			# Barometric pressure, already temp. comp.
-float32 baro_alt_meter			# Altitude, already temp. comp.
-float32 baro_temp_celcius		# Temperature in degrees celsius
-uint64 baro_timestamp		# Barometer timestamp
-uint32 baro_priority			# Sensor priority
-
-float32 baro1_pres_mbar			# Barometric pressure, already temp. comp.
-float32 baro1_alt_meter			# Altitude, already temp. comp.
-float32 baro1_temp_celcius		# Temperature in degrees celsius
-uint64 baro1_timestamp		# Barometer timestamp
-uint32 baro1_priority			# Sensor priority
+float32[3] baro_pres_mbar			# Barometric pressure, already temp. comp.
+float32[3] baro_alt_meter			# Altitude, already temp. comp.
+float32[3] baro_temp_celcius		# Temperature in degrees celsius
+uint64[3] baro_timestamp		# Barometer timestamp
+uint32[3] baro_priority			# Sensor priority
+uint32[3] baro_errcount			# Error count in communication
 
 float32[10] adc_voltage_v		# ADC voltages of ADC Chan 10/11/12/13 or -1
 uint16[10] adc_mapping		# Channel indices of each of these values
 float32 mcu_temp_celcius			# Internal temperature measurement of MCU
 
-float32 differential_pressure_pa			# Airspeed sensor differential pressure
-uint64 differential_pressure_timestamp	# Last measurement timestamp
-float32 differential_pressure_filtered_pa	# Low pass filtered airspeed sensor differential pressure reading
-uint32 differential_pressure_priority			# Sensor priority
-
-float32 differential_pressure1_pa			# Airspeed sensor differential pressure
-uint64 differential_pressure1_timestamp	# Last measurement timestamp
-float32 differential_pressure1_filtered_pa	# Low pass filtered airspeed sensor differential pressure reading
-uint32 differential_pressure1_priority			# Sensor priority
+float32[3] differential_pressure_pa			# Airspeed sensor differential pressure
+uint64[3] differential_pressure_timestamp	# Last measurement timestamp
+float32[3] differential_pressure_filtered_pa	# Low pass filtered airspeed sensor differential pressure reading
+uint32[3] differential_pressure_priority			# Sensor priority
+uint32[3] differential_pressure_errcount			# Error count in communication

msg/sensor_gyro.msg

@@ -1,8 +1,12 @@
 uint64 timestamp
+uint64 integral_dt	# integration time
 uint64 error_count
 float32 x		# angular velocity in the NED X board axis in rad/s
 float32 y		# angular velocity in the NED Y board axis in rad/s
 float32 z		# angular velocity in the NED Z board axis in rad/s
+float32 x_integral	# delta angle in the NED X board axis in rad/s in the integration time frame
+float32 y_integral	# delta angle in the NED Y board axis in rad/s in the integration time frame
+float32 z_integral	# delta angle in the NED Z board axis in rad/s in the integration time frame
 float32 temperature	# temperature in degrees celcius
 float32 range_rad_s
 float32 scaling

msg/vehicle_attitude.msg

@@ -10,6 +10,9 @@ float32 yawspeed	# Yaw body angular rate (rad/s, x forward/y right/z down)
 float32 rollacc		# Roll angular accelration (rad/s^2, x forward/y right/z down)
 float32 pitchacc	# Pitch angular acceleration (rad/s^2, x forward/y right/z down)
 float32 yawacc		# Yaw angular acceleration (rad/s^2, x forward/y right/z down)
+float32 rate_vibration	# Value between 0 and 1 indicating vibration. A value of 0 means no vibration, a value of 1 indicates unbearable vibration levels.
+float32 accel_vibration	# Value between 0 and 1 indicating vibration. A value of 0 means no vibration, a value of 1 indicates unbearable vibration levels.
+float32 mag_vibration	# Value between 0 and 1 indicating vibration. A value of 0 means no vibration, a value of 1 indicates unbearable vibration levels.
 float32[3] rate_offsets	# Offsets of the body angular rates from zero
 float32[9] R		# Rotation matrix, body to world, (Tait-Bryan, NED)
 float32[4] q		# Quaternion (NED)

nuttx-configs/px4fmu-v2/nsh/defconfig

@@ -551,8 +551,8 @@ CONFIG_UART7_SERIAL_CONSOLE=y
 #
 # USART1 Configuration
 #
-CONFIG_USART1_RXBUFSIZE=600
-CONFIG_USART1_TXBUFSIZE=600
+CONFIG_USART1_RXBUFSIZE=128
+CONFIG_USART1_TXBUFSIZE=32
 CONFIG_USART1_BAUD=115200
 CONFIG_USART1_BITS=8
 CONFIG_USART1_PARITY=0

posix-configs/SITL/init/rc.fixed_wing

@@ -4,7 +4,6 @@ param load
 param set MAV_TYPE 1
 param set SYS_AUTOSTART 3033
 param set SYS_RESTART_TYPE 2
-param set COM_RC_IN_MODE 2
 dataman start
 param set CAL_GYRO0_ID 2293760
 param set CAL_ACC0_ID 1376256
@@ -22,6 +21,7 @@ param set CAL_MAG0_XOFF 0.01
 param set MPC_XY_P 0.4
 param set MPC_XY_VEL_P 0.2
 param set MPC_XY_VEL_D 0.005
+param set COM_RC_IN_MODE 2
 rgbled start
 tone_alarm start
 gyrosim start
@@ -29,9 +29,11 @@ accelsim start
 barosim start
 adcsim start
 gpssim start
+measairspeedsim start
 pwm_out_sim mode_pwm
-commander start
+sleep 1
 sensors start
+commander start
 land_detector start fixedwing
 navigator start
 ekf_att_pos_estimator start
@@ -44,3 +46,4 @@ mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556
 mavlink stream -r 50 -s ATTITUDE -u 14556
 mavlink stream -r 50 -s ATTITUDE_TARGET -u 14556
 mavlink boot_complete
+sdlog2 start -r 100 -e -t -a

posix-configs/SITL/init/rcS

@@ -8,7 +8,6 @@ param set MC_YAW_P 2.0
 param set MC_YAWRATE_P 0.35
 param set SYS_AUTOSTART 4010
 param set SYS_RESTART_TYPE 2
-param set COM_RC_IN_MODE 2
 dataman start
 param set CAL_GYRO0_ID 2293760
 param set CAL_ACC0_ID 1376256
@@ -27,6 +26,7 @@ param set MPC_XY_P 0.4
 param set MPC_XY_VEL_P 0.2
 param set MPC_XY_VEL_D 0.005
 param set SENS_BOARD_ROT 0
+param set COM_RC_IN_MODE 2
 rgbled start
 tone_alarm start
 gyrosim start
@@ -35,8 +35,9 @@ barosim start
 adcsim start
 gpssim start
 pwm_out_sim mode_pwm
-commander start
+sleep 1
 sensors start
+commander start
 land_detector start multicopter
 navigator start
 attitude_estimator_q start
@@ -52,4 +53,6 @@ mavlink stream -r 80 -s ATTITUDE -u 14556
 mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556
 mavlink stream -r 20 -s RC_CHANNELS -u 14556
 mavlink stream -r 250 -s HIGHRES_IMU -u 14556
+mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
 mavlink boot_complete
+sdlog2 start -r 100 -e -t -a

posix-configs/SITL/init/rc_iris_ros

@@ -8,7 +8,6 @@ param set MC_YAW_P 2.0
 param set MC_YAWRATE_P 0.35
 param set SYS_AUTOSTART 4010
 param set SYS_RESTART_TYPE 2
-param set COM_RC_IN_MODE 2
 dataman start
 param set CAL_GYRO0_ID 2293760
 param set CAL_ACC0_ID 1376256
@@ -41,6 +40,7 @@ param set MP_ROLLRATE_I 0.001
 param set MP_ROLLRATE_D 0.001
 param set MP_PITCH_P 4
 param set MP_PITCHRATE_P 0.3
+param set COM_RC_IN_MODE 2
 rgbled start
 tone_alarm start
 gyrosim start
@@ -49,8 +49,9 @@ barosim start
 adcsim start
 gpssim start
 pwm_out_sim mode_pwm
-commander start
+sleep 1
 sensors start
+commander start
 land_detector start multicopter
 navigator start
 attitude_estimator_q start

src/drivers/blinkm/blinkm.cpp

@@ -244,7 +244,7 @@ private:
 /* for now, we only support one BlinkM */
 namespace
 {
-	BlinkM *g_blinkm;
+BlinkM *g_blinkm;
 }
 
 /* list of script names, must match script ID numbers */
@@ -277,9 +277,9 @@ extern "C" __EXPORT int blinkm_main(int argc, char *argv[]);
 BlinkM::BlinkM(int bus, int blinkm) :
 	I2C("blinkm", BLINKM0_DEVICE_PATH, bus, blinkm
 #ifdef __PX4_NUTTX
-		, 100000
+	    , 100000
 #endif
-		),
+	   ),
 	led_color_1(LED_OFF),
 	led_color_2(LED_OFF),
 	led_color_3(LED_OFF),
@@ -325,7 +325,7 @@ BlinkM::init()
 	}
 
 	stop_script();
-	set_rgb(0,0,0);
+	set_rgb(0, 0, 0);
 
 	return OK;
 }
@@ -333,13 +333,14 @@ BlinkM::init()
 int
 BlinkM::setMode(int mode)
 {
-	if(mode == 1) {
-		if(systemstate_run == false) {
+	if (mode == 1) {
+		if (systemstate_run == false) {
 			stop_script();
-			set_rgb(0,0,0);
+			set_rgb(0, 0, 0);
 			systemstate_run = true;
 			work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, 1);
 		}
+
 	} else {
 		systemstate_run = false;
 	}
@@ -355,8 +356,9 @@ BlinkM::probe()
 
 	ret = get_firmware_version(version);
 
-	if (ret == OK)
+	if (ret == OK) {
 		DEVICE_DEBUG("found BlinkM firmware version %c%c", version[1], version[0]);
+	}
 
 	return ret;
 }
@@ -372,6 +374,7 @@ BlinkM::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 			ret = EINVAL;
 			break;
 		}
+
 		ret = play_script((const char *)arg);
 		break;
 
@@ -380,25 +383,31 @@ BlinkM::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 		break;
 
 	case BLINKM_SET_USER_SCRIPT: {
-		if (arg == 0) {
-			ret = EINVAL;
+			if (arg == 0) {
+				ret = EINVAL;
+				break;
+			}
+
+			unsigned lines = 0;
+			const uint8_t *script = (const uint8_t *)arg;
+
+			while ((lines < 50) && (script[1] != 0)) {
+				ret = write_script_line(lines, script[0], script[1], script[2], script[3], script[4]);
+
+				if (ret != OK) {
+					break;
+				}
+
+				script += 5;
+			}
+
+			if (ret == OK) {
+				ret = set_script(lines, 0);
+			}
+
 			break;
 		}
 
-		unsigned lines = 0;
-		const uint8_t *script = (const uint8_t *)arg;
-
-		while ((lines < 50) && (script[1] != 0)) {
-			ret = write_script_line(lines, script[0], script[1], script[2], script[3], script[4]);
-			if (ret != OK)
-				break;
-			script += 5;
-		}
-		if (ret == OK)
-			ret = set_script(lines, 0);
-		break;
-	}
-
 	default:
 		break;
 	}
@@ -421,7 +430,7 @@ void
 BlinkM::led()
 {
 
-	if(!topic_initialized) {
+	if (!topic_initialized) {
 		vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
 		orb_set_interval(vehicle_status_sub_fd, 250);
 
@@ -441,29 +450,36 @@ BlinkM::led()
 		topic_initialized = true;
 	}
 
-	if(led_thread_ready == true) {
-		if(!detected_cells_blinked) {
-			if(num_of_cells > 0) {
+	if (led_thread_ready == true) {
+		if (!detected_cells_blinked) {
+			if (num_of_cells > 0) {
 				t_led_color[0] = LED_PURPLE;
 			}
-			if(num_of_cells > 1) {
+
+			if (num_of_cells > 1) {
 				t_led_color[1] = LED_PURPLE;
 			}
-			if(num_of_cells > 2) {
+
+			if (num_of_cells > 2) {
 				t_led_color[2] = LED_PURPLE;
 			}
-			if(num_of_cells > 3) {
+
+			if (num_of_cells > 3) {
 				t_led_color[3] = LED_PURPLE;
 			}
-			if(num_of_cells > 4) {
+
+			if (num_of_cells > 4) {
 				t_led_color[4] = LED_PURPLE;
 			}
-			if(num_of_cells > 5) {
+
+			if (num_of_cells > 5) {
 				t_led_color[5] = LED_PURPLE;
 			}
+
 			t_led_color[6] = LED_OFF;
 			t_led_color[7] = LED_OFF;
 			t_led_blink = LED_BLINK;
+
 		} else {
 			t_led_color[0] = led_color_1;
 			t_led_color[1] = led_color_2;
@@ -475,13 +491,17 @@ BlinkM::led()
 			t_led_color[7] = led_color_8;
 			t_led_blink = led_blink;
 		}
+
 		led_thread_ready = false;
 	}
 
 	if (led_thread_runcount & 1) {
-		if (t_led_blink)
+		if (t_led_blink) {
 			setLEDColor(LED_OFF);
+		}
+
 		led_interval = LED_OFFTIME;
+
 	} else {
 		setLEDColor(t_led_color[(led_thread_runcount / 2) % 8]);
 		//led_interval = (led_thread_runcount & 1) : LED_ONTIME;
@@ -516,10 +536,13 @@ BlinkM::led()
 		if (new_data_vehicle_status) {
 			orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw);
 			no_data_vehicle_status = 0;
+
 		} else {
 			no_data_vehicle_status++;
-			if(no_data_vehicle_status >= 3)
+
+			if (no_data_vehicle_status >= 3) {
 				no_data_vehicle_status = 3;
+			}
 		}
 
 		orb_check(vehicle_control_mode_sub_fd, &new_data_vehicle_control_mode);
@@ -527,10 +550,13 @@ BlinkM::led()
 		if (new_data_vehicle_control_mode) {
 			orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub_fd, &vehicle_control_mode);
 			no_data_vehicle_control_mode = 0;
+
 		} else {
 			no_data_vehicle_control_mode++;
-			if(no_data_vehicle_control_mode >= 3)
+
+			if (no_data_vehicle_control_mode >= 3) {
 				no_data_vehicle_control_mode = 3;
+			}
 		}
 
 		orb_check(actuator_armed_sub_fd, &new_data_actuator_armed);
@@ -538,10 +564,13 @@ BlinkM::led()
 		if (new_data_actuator_armed) {
 			orb_copy(ORB_ID(actuator_armed), actuator_armed_sub_fd, &actuator_armed);
 			no_data_actuator_armed = 0;
+
 		} else {
 			no_data_actuator_armed++;
-			if(no_data_actuator_armed >= 3)
+
+			if (no_data_actuator_armed >= 3) {
 				no_data_actuator_armed = 3;
+			}
 		}
 
 		orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position);
@@ -549,10 +578,13 @@ BlinkM::led()
 		if (new_data_vehicle_gps_position) {
 			orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub_fd, &vehicle_gps_position_raw);
 			no_data_vehicle_gps_position = 0;
+
 		} else {
 			no_data_vehicle_gps_position++;
-			if(no_data_vehicle_gps_position >= 3)
+
+			if (no_data_vehicle_gps_position >= 3) {
 				no_data_vehicle_gps_position = 3;
+			}
 		}
 
 		/* update safety topic */
@@ -569,13 +601,15 @@ BlinkM::led()
 			if (num_of_cells == 0) {
 				/* looking for lipo cells that are connected */
 				printf("<blinkm> checking cells\n");
-				for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) {
-					if(vehicle_status_raw.battery_voltage < num_of_cells * MAX_CELL_VOLTAGE) break;
+
+				for (num_of_cells = 2; num_of_cells < 7; num_of_cells++) {
+					if (vehicle_status_raw.battery_voltage < num_of_cells * MAX_CELL_VOLTAGE) { break; }
 				}
+
 				printf("<blinkm> cells found:%d\n", num_of_cells);
 
 			} else {
-				if(vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) {
+				if (vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) {
 					/* LED Pattern for battery critical alerting */
 					led_color_1 = LED_RED;
 					led_color_2 = LED_RED;
@@ -587,7 +621,7 @@ BlinkM::led()
 					led_color_8 = LED_RED;
 					led_blink = LED_BLINK;
 
-				} else if(vehicle_status_raw.rc_signal_lost) {
+				} else if (vehicle_status_raw.rc_signal_lost) {
 					/* LED Pattern for FAILSAFE */
 					led_color_1 = LED_BLUE;
 					led_color_2 = LED_BLUE;
@@ -599,7 +633,7 @@ BlinkM::led()
 					led_color_8 = LED_BLUE;
 					led_blink = LED_BLINK;
 
-				} else if(vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW) {
+				} else if (vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW) {
 					/* LED Pattern for battery low warning */
 					led_color_1 = LED_YELLOW;
 					led_color_2 = LED_YELLOW;
@@ -614,9 +648,9 @@ BlinkM::led()
 				} else {
 					/* no battery warnings here */
 
-					if(actuator_armed.armed == false) {
+					if (actuator_armed.armed == false) {
 						/* system not armed */
-						if(safety.safety_off){
+						if (safety.safety_off) {
 							led_color_1 = LED_ORANGE;
 							led_color_2 = LED_ORANGE;
 							led_color_3 = LED_ORANGE;
@@ -626,7 +660,8 @@ BlinkM::led()
 							led_color_7 = LED_ORANGE;
 							led_color_8 = LED_ORANGE;
 							led_blink = LED_BLINK;
-						}else{
+
+						} else {
 							led_color_1 = LED_CYAN;
 							led_color_2 = LED_CYAN;
 							led_color_3 = LED_CYAN;
@@ -637,6 +672,7 @@ BlinkM::led()
 							led_color_8 = LED_CYAN;
 							led_blink = LED_NOBLINK;
 						}
+
 					} else {
 						/* armed system - initial led pattern */
 						led_color_1 = LED_RED;
@@ -649,32 +685,41 @@ BlinkM::led()
 						led_color_8 = LED_OFF;
 						led_blink = LED_BLINK;
 
-						if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) {
+						if (new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) {
 							/* indicate main control state */
-							if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_POSCTL)
+							if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_POSCTL) {
 								led_color_4 = LED_GREEN;
+							}
+
 							/* TODO: add other Auto modes */
-							else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_AUTO_MISSION)
+							else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_AUTO_MISSION) {
 								led_color_4 = LED_BLUE;
-							else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_ALTCTL)
+
+							} else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_ALTCTL) {
 								led_color_4 = LED_YELLOW;
-							else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_MANUAL)
+
+							} else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_MANUAL) {
 								led_color_4 = LED_WHITE;
-							else
+
+							} else {
 								led_color_4 = LED_OFF;
+							}
+
 							led_color_5 = led_color_4;
 						}
 
-						if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) {
+						if (new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) {
 							/* handling used satus */
-							if(num_of_used_sats >= 7) {
+							if (num_of_used_sats >= 7) {
 								led_color_1 = LED_OFF;
 								led_color_2 = LED_OFF;
 								led_color_3 = LED_OFF;
-							} else if(num_of_used_sats == 6) {
+
+							} else if (num_of_used_sats == 6) {
 								led_color_2 = LED_OFF;
 								led_color_3 = LED_OFF;
-							} else if(num_of_used_sats == 5) {
+
+							} else if (num_of_used_sats == 5) {
 								led_color_3 = LED_OFF;
 							}
 
@@ -689,6 +734,7 @@ BlinkM::led()
 					}
 				}
 			}
+
 		} else {
 			/* LED Pattern for general Error - no vehicle_status can retrieved */
 			led_color_1 = LED_WHITE;
@@ -716,12 +762,13 @@ BlinkM::led()
 		vehicle_gps_position_raw.satellites_visible);
 		*/
 
-		led_thread_runcount=0;
+		led_thread_runcount = 0;
 		led_thread_ready = true;
 		led_interval = LED_OFFTIME;
 
-		if(detected_cells_runcount < 4){
+		if (detected_cells_runcount < 4) {
 			detected_cells_runcount++;
+
 		} else {
 			detected_cells_blinked = true;
 		}
@@ -730,47 +777,58 @@ BlinkM::led()
 		led_thread_runcount++;
 	}
 
-	if(systemstate_run == true) {
+	if (systemstate_run == true) {
 		/* re-queue ourselves to run again later */
 		work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, led_interval);
+
 	} else {
 		stop_script();
-		set_rgb(0,0,0);
+		set_rgb(0, 0, 0);
 	}
 }
 
-void BlinkM::setLEDColor(int ledcolor) {
+void BlinkM::setLEDColor(int ledcolor)
+{
 	switch (ledcolor) {
-		case LED_OFF:	// off
-			set_rgb(0,0,0);
-			break;
-		case LED_RED:	// red
-			set_rgb(255,0,0);
-			break;
-		case LED_ORANGE:	// orange
-			set_rgb(255,150,0);
-			break;
-		case LED_YELLOW:	// yellow
-			set_rgb(200,200,0);
-			break;
-		case LED_PURPLE:	// purple
-			set_rgb(255,0,255);
-			break;
-		case LED_GREEN:	// green
-			set_rgb(0,255,0);
-			break;
-		case LED_BLUE:	// blue
-			set_rgb(0,0,255);
-			break;
-		case LED_CYAN:	// cyan
-			set_rgb(0,128,128);
-			break;
-		case LED_WHITE:	// white
-			set_rgb(255,255,255);
-			break;
-		case LED_AMBER:	// amber
-			set_rgb(255,65,0);
-			break;
+	case LED_OFF:	// off
+		set_rgb(0, 0, 0);
+		break;
+
+	case LED_RED:	// red
+		set_rgb(255, 0, 0);
+		break;
+
+	case LED_ORANGE:	// orange
+		set_rgb(255, 150, 0);
+		break;
+
+	case LED_YELLOW:	// yellow
+		set_rgb(200, 200, 0);
+		break;
+
+	case LED_PURPLE:	// purple
+		set_rgb(255, 0, 255);
+		break;
+
+	case LED_GREEN:	// green
+		set_rgb(0, 255, 0);
+		break;
+
+	case LED_BLUE:	// blue
+		set_rgb(0, 0, 255);
+		break;
+
+	case LED_CYAN:	// cyan
+		set_rgb(0, 128, 128);
+		break;
+
+	case LED_WHITE:	// white
+		set_rgb(255, 255, 255);
+		break;
+
+	case LED_AMBER:	// amber
+		set_rgb(255, 65, 0);
+		break;
 	}
 }
 
@@ -855,8 +913,9 @@ BlinkM::play_script(const char *script_name)
 	}
 
 	for (unsigned i = 0; script_names[i] != nullptr; i++)
-		if (!strcasecmp(script_name, script_names[i]))
+		if (!strcasecmp(script_name, script_names[i])) {
 			return play_script(i);
+		}
 
 	return -1;
 }
@@ -931,7 +990,8 @@ BlinkM::get_firmware_version(uint8_t version[2])
 
 void blinkm_usage();
 
-void blinkm_usage() {
+void blinkm_usage()
+{
 	warnx("missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}");
 	warnx("options:");
 	warnx("\t-b --bus i2cbus (3)");
@@ -951,6 +1011,7 @@ blinkm_main(int argc, char *argv[])
 		blinkm_usage();
 		return 1;
 	}
+
 	for (x = 1; x < argc; x++) {
 		if (strcmp(argv[x], "-b") == 0 || strcmp(argv[x], "--bus") == 0) {
 			if (argc > x + 1) {
@@ -1008,22 +1069,27 @@ blinkm_main(int argc, char *argv[])
 
 
 	if (!strcmp(argv[1], "list")) {
-		for (unsigned i = 0; BlinkM::script_names[i] != nullptr; i++)
+		for (unsigned i = 0; BlinkM::script_names[i] != nullptr; i++) {
 			fprintf(stderr, "    %s\n", BlinkM::script_names[i]);
+		}
+
 		fprintf(stderr, "    <html color number>\n");
 		return 0;
 	}
 
 	/* things that require access to the device */
 	int fd = px4_open(BLINKM0_DEVICE_PATH, 0);
+
 	if (fd < 0) {
 		warn("can't open BlinkM device");
 		return 1;
 	}
 
 	g_blinkm->setMode(0);
-	if (px4_ioctl(fd, BLINKM_PLAY_SCRIPT_NAMED, (unsigned long)argv[1]) == OK)
+
+	if (px4_ioctl(fd, BLINKM_PLAY_SCRIPT_NAMED, (unsigned long)argv[1]) == OK) {
 		return 0;
+	}
 
 	px4_close(fd);
 

src/drivers/bma180/bma180.cpp

@@ -262,8 +262,9 @@ BMA180::~BMA180()
 	stop();
 
 	/* free any existing reports */
-	if (_reports != nullptr)
+	if (_reports != nullptr) {
 		delete _reports;
+	}
 
 	/* delete the perf counter */
 	perf_free(_sample_perf);
@@ -275,14 +276,16 @@ BMA180::init()
 	int ret = ERROR;
 
 	/* do SPI init (and probe) first */
-	if (SPI::init() != OK)
+	if (SPI::init() != OK) {
 		goto out;
+	}
 
 	/* allocate basic report buffers */
 	_reports = new RingBuffer(2, sizeof(accel_report));
 
-	if (_reports == nullptr)
+	if (_reports == nullptr) {
 		goto out;
+	}
 
 	/* perform soft reset (p48) */
 	write_reg(ADDR_RESET, SOFT_RESET);
@@ -308,9 +311,9 @@ BMA180::init()
 	/* disable writing to chip config */
 	modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0);
 
-	if (set_range(4)) warnx("Failed setting range");
+	if (set_range(4)) { warnx("Failed setting range"); }
 
-	if (set_lowpass(75)) warnx("Failed setting lowpass");
+	if (set_lowpass(75)) { warnx("Failed setting lowpass"); }
 
 	if (read_reg(ADDR_CHIP_ID) == CHIP_ID) {
 		ret = OK;
@@ -342,8 +345,9 @@ BMA180::probe()
 	/* dummy read to ensure SPI state machine is sane */
 	read_reg(ADDR_CHIP_ID);
 
-	if (read_reg(ADDR_CHIP_ID) == CHIP_ID)
+	if (read_reg(ADDR_CHIP_ID) == CHIP_ID) {
 		return OK;
+	}
 
 	return -EIO;
 }
@@ -356,8 +360,9 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen)
 	int ret = 0;
 
 	/* buffer must be large enough */
-	if (count < 1)
+	if (count < 1) {
 		return -ENOSPC;
+	}
 
 	/* if automatic measurement is enabled */
 	if (_call_interval > 0) {
@@ -383,8 +388,9 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen)
 	measure();
 
 	/* measurement will have generated a report, copy it out */
-	if (_reports->get(arp))
+	if (_reports->get(arp)) {
 		ret = sizeof(*arp);
+	}
 
 	return ret;
 }
@@ -397,27 +403,27 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case SENSORIOCSPOLLRATE: {
 			switch (arg) {
 
-				/* switching to manual polling */
+			/* switching to manual polling */
 			case SENSOR_POLLRATE_MANUAL:
 				stop();
 				_call_interval = 0;
 				return OK;
 
-				/* external signalling not supported */
+			/* external signalling not supported */
 			case SENSOR_POLLRATE_EXTERNAL:
 
-				/* zero would be bad */
+			/* zero would be bad */
 			case 0:
 				return -EINVAL;
 
 
-				/* set default/max polling rate */
+			/* set default/max polling rate */
 			case SENSOR_POLLRATE_MAX:
 			case SENSOR_POLLRATE_DEFAULT:
 				/* With internal low pass filters enabled, 250 Hz is sufficient */
 				return ioctl(filp, SENSORIOCSPOLLRATE, 250);
 
-				/* adjust to a legal polling interval in Hz */
+			/* adjust to a legal polling interval in Hz */
 			default: {
 					/* do we need to start internal polling? */
 					bool want_start = (_call_interval == 0);
@@ -426,16 +432,18 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg)
 					unsigned ticks = 1000000 / arg;
 
 					/* check against maximum sane rate */
-					if (ticks < 1000)
+					if (ticks < 1000) {
 						return -EINVAL;
+					}
 
 					/* update interval for next measurement */
 					/* XXX this is a bit shady, but no other way to adjust... */
 					_call.period = _call_interval = ticks;
 
 					/* if we need to start the poll state machine, do it */
-					if (want_start)
+					if (want_start) {
 						start();
+					}
 
 					return OK;
 				}
@@ -443,25 +451,29 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg)
 		}
 
 	case SENSORIOCGPOLLRATE:
-		if (_call_interval == 0)
+		if (_call_interval == 0) {
 			return SENSOR_POLLRATE_MANUAL;
+		}
 
 		return 1000000 / _call_interval;
 
 	case SENSORIOCSQUEUEDEPTH: {
-		/* lower bound is mandatory, upper bound is a sanity check */
-		if ((arg < 2) || (arg > 100))
-			return -EINVAL;
-		
-		irqstate_t flags = irqsave();
-		if (!_reports->resize(arg)) {
+			/* lower bound is mandatory, upper bound is a sanity check */
+			if ((arg < 2) || (arg > 100)) {
+				return -EINVAL;
+			}
+
+			irqstate_t flags = irqsave();
+
+			if (!_reports->resize(arg)) {
+				irqrestore(flags);
+				return -ENOMEM;
+			}
+
 			irqrestore(flags);
-			return -ENOMEM;
+
+			return OK;
 		}
-		irqrestore(flags);
-		
-		return OK;
-	}
 
 	case SENSORIOCGQUEUEDEPTH:
 		return _reports->size();
@@ -543,11 +555,13 @@ BMA180::set_range(unsigned max_g)
 {
 	uint8_t rangebits;
 
-	if (max_g == 0)
+	if (max_g == 0) {
 		max_g = 16;
+	}
 
-	if (max_g > 16)
+	if (max_g > 16) {
 		return -ERANGE;
+	}
 
 	if (max_g <= 2) {
 		_current_range = 2;
@@ -700,7 +714,7 @@ BMA180::measure()
 	 * measurement flow without using the external interrupt.
 	 */
 	report.timestamp = hrt_absolute_time();
-        report.error_count = 0;
+	report.error_count = 0;
 	/*
 	 * y of board is x of sensor and x of board is -y of sensor
 	 * perform only the axis assignment here.
@@ -733,8 +747,9 @@ BMA180::measure()
 	poll_notify(POLLIN);
 
 	/* publish for subscribers */
-	if (_accel_topic != nullptr && !(_pub_blocked))
+	if (_accel_topic != nullptr && !(_pub_blocked)) {
 		orb_publish(ORB_ID(sensor_accel), _accel_topic, &report);
+	}
 
 	/* stop the perf counter */
 	perf_end(_sample_perf);
@@ -768,26 +783,31 @@ start()
 {
 	int fd;
 
-	if (g_dev != nullptr)
+	if (g_dev != nullptr) {
 		errx(1, "already started");
+	}
 
 	/* create the driver */
 	g_dev = new BMA180(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_ACCEL);
 
-	if (g_dev == nullptr)
+	if (g_dev == nullptr) {
 		goto fail;
+	}
 
-	if (OK != g_dev->init())
+	if (OK != g_dev->init()) {
 		goto fail;
+	}
 
 	/* set the poll rate to default, starts automatic data collection */
 	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
 
-	if (fd < 0)
+	if (fd < 0) {
 		goto fail;
+	}
 
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
 		goto fail;
+	}
 
 	exit(0);
 fail:
@@ -820,14 +840,16 @@ test()
 		    ACCEL_DEVICE_PATH);
 
 	/* reset to manual polling */
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) {
 		err(1, "reset to manual polling");
+	}
 
 	/* do a simple demand read */
 	sz = read(fd, &a_report, sizeof(a_report));
 
-	if (sz != sizeof(a_report))
+	if (sz != sizeof(a_report)) {
 		err(1, "immediate acc read failed");
+	}
 
 	warnx("single read");
 	warnx("time:     %lld", a_report.timestamp);
@@ -854,14 +876,17 @@ reset()
 {
 	int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
 
-	if (fd < 0)
+	if (fd < 0) {
 		err(1, "failed ");
+	}
 
-	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
 		err(1, "driver reset failed");
+	}
 
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
 		err(1, "driver poll restart failed");
+	}
 
 	exit(0);
 }
@@ -872,8 +897,9 @@ reset()
 void
 info()
 {
-	if (g_dev == nullptr)
+	if (g_dev == nullptr) {
 		errx(1, "BMA180: driver not running");
+	}
 
 	printf("state @ %p\n", g_dev);
 	g_dev->print_info();
@@ -891,26 +917,30 @@ bma180_main(int argc, char *argv[])
 	 * Start/load the driver.
 
 	 */
-	if (!strcmp(argv[1], "start"))
+	if (!strcmp(argv[1], "start")) {
 		bma180::start();
+	}
 
 	/*
 	 * Test the driver/device.
 	 */
-	if (!strcmp(argv[1], "test"))
+	if (!strcmp(argv[1], "test")) {
 		bma180::test();
+	}
 
 	/*
 	 * Reset the driver.
 	 */
-	if (!strcmp(argv[1], "reset"))
+	if (!strcmp(argv[1], "reset")) {
 		bma180::reset();
+	}
 
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(argv[1], "info"))
+	if (!strcmp(argv[1], "info")) {
 		bma180::info();
+	}
 
 	errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'");
 }

src/drivers/boards/sitl/module.mk

@@ -0,0 +1,8 @@
+#
+# Board-specific startup code for SITL
+#
+
+SRCS		 =  \
+		   sitl_led.c
+
+MAXOPTIMIZATION	 = -Os

src/drivers/boards/sitl/sitl_led.c

@@ -0,0 +1,81 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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 sitl_led.c
+ *
+ * sitl LED backend.
+ */
+
+#include <px4_config.h>
+#include <px4_log.h>
+#include <stdbool.h>
+
+__BEGIN_DECLS
+extern void led_init(void);
+extern void led_on(int led);
+extern void led_off(int led);
+extern void led_toggle(int led);
+__END_DECLS
+
+static bool _led_state[2] = { false , false };
+
+__EXPORT void led_init()
+{
+	PX4_DEBUG("LED_INIT");
+}
+
+__EXPORT void led_on(int led)
+{
+	if (led == 1 || led == 0) {
+		PX4_DEBUG("LED%d_ON", led);
+		_led_state[led] = true;
+	}
+}
+
+__EXPORT void led_off(int led)
+{
+	if (led == 1 || led == 0) {
+		PX4_DEBUG("LED%d_OFF", led);
+		_led_state[led] = false;
+	}
+}
+
+__EXPORT void led_toggle(int led)
+{
+	if (led == 1 || led == 0) {
+		_led_state[led] = !_led_state[led];
+		PX4_DEBUG("LED%d_TOGGLE: %s", led, _led_state[led] ? "ON" : "OFF");
+
+	}
+}

src/drivers/device/integrator.h

@@ -0,0 +1,183 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 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 integrator.h
+ *
+ * A resettable integrator
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ */
+
+#pragma once
+
+#include <mathlib/mathlib.h>
+
+#include <mathlib/mathlib.h>
+
+class Integrator {
+public:
+	Integrator(uint64_t auto_reset_interval = 4000 /* 250 Hz */, bool coning_compensation = false);
+	virtual ~Integrator();
+
+	/**
+	 * Put an item into the integral.
+	 *
+	 * @param timestamp	Timestamp of the current value
+	 * @param val		Item to put
+	 * @param integral	Current integral in case the integrator did reset, else the value will not be modified
+	 * @return		true if putting the item triggered an integral reset
+	 *			and the integral should be published
+	 */
+	bool			put(hrt_abstime timestamp, math::Vector<3> &val, math::Vector<3> &integral, uint64_t &integral_dt);
+
+	/**
+	 * Get the current integral value
+	 *
+	 * @return		the integral since the last auto-reset
+	 */
+	math::Vector<3>		get() { return _integral_auto; }
+
+	/**
+	 * Read from the integral
+	 *
+	 * @param auto_reset	Reset the integral to zero on read
+	 * @return		the integral since the last read-reset
+	 */
+	math::Vector<3>		read(bool auto_reset);
+
+	/**
+	 * Get current integral start time
+	 */
+	hrt_abstime		current_integral_start() { return _last_auto; }
+
+private:
+	hrt_abstime _auto_reset_interval;		/**< the interval after which the content will be published and the integrator reset */
+	hrt_abstime _last_integration;			/**< timestamp of the last integration step */
+	hrt_abstime _last_auto;				/**< last auto-announcement of integral value */
+	math::Vector<3> _integral_auto;			/**< the integrated value which auto-resets after _auto_reset_interval */
+	math::Vector<3> _integral_read;			/**< the integrated value since the last read */
+	math::Vector<3> _last_val;			/**< previously integrated last value */
+	math::Vector<3> _last_delta;			/**< last local delta */
+	void (*_auto_callback)(hrt_abstime, math::Vector<3>);	/**< the function callback for auto-reset */
+	bool _coning_comp_on;				/**< coning compensation */
+
+	/* we don't want this class to be copied */
+	Integrator(const Integrator&);
+	Integrator operator=(const Integrator&);
+};
+
+Integrator::Integrator(hrt_abstime auto_reset_interval, bool coning_compensation) :
+	_auto_reset_interval(auto_reset_interval),
+	_last_integration(0),
+	_last_auto(0),
+	_integral_auto(0.0f, 0.0f, 0.0f),
+	_integral_read(0.0f, 0.0f, 0.0f),
+	_last_val(0.0f, 0.0f, 0.0f),
+	_last_delta(0.0f, 0.0f, 0.0f),
+	_auto_callback(nullptr),
+	_coning_comp_on(coning_compensation)
+{
+
+}
+
+Integrator::~Integrator()
+{
+
+}
+
+bool
+Integrator::put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integral, uint64_t &integral_dt)
+{
+	bool auto_reset = false;
+
+	if (_last_integration == 0) {
+		/* this is the first item in the integrator */
+		_last_integration = timestamp;
+		_last_auto = timestamp;
+		_last_val = val;
+		return false;
+	}
+
+	// Integrate
+	double dt = (double)(timestamp - _last_integration) / 1000000.0;
+	math::Vector<3> i = (val + _last_val) * dt * 0.5f;
+
+	// Apply coning compensation if required
+	if (_coning_comp_on) {
+		// Coning compensation derived by Paul Riseborough and Jonathan Challinger,
+		// following:
+		// Tian et al (2010) Three-loop Integration of GPS and Strapdown INS with Coning and Sculling Compensation
+     		// Available: http://www.sage.unsw.edu.au/snap/publications/tian_etal2010b.pdf
+
+		i += ((_integral_auto + _last_delta * (1.0f / 6.0f)) % i) * 0.5f;
+	}
+
+	_integral_auto += i;
+	_integral_read += i;
+
+	_last_integration = timestamp;
+	_last_val = val;
+	_last_delta = i;
+
+	if ((timestamp - _last_auto) > _auto_reset_interval) {
+		if (_auto_callback) {
+			/* call the callback */
+			_auto_callback(timestamp, _integral_auto);
+		}
+
+		integral = _integral_auto;
+		integral_dt = (timestamp - _last_auto);
+
+		auto_reset = true;
+		_last_auto = timestamp;
+		_integral_auto(0) = 0.0f;
+		_integral_auto(1) = 0.0f;
+		_integral_auto(2) = 0.0f;
+	}
+
+	return auto_reset;
+}
+
+math::Vector<3>
+Integrator::read(bool auto_reset)
+{
+	math::Vector<3> val = _integral_read;
+	if (auto_reset) {
+		_integral_read(0) = 0.0f;
+		_integral_read(1) = 0.0f;
+		_integral_read(2) = 0.0f;
+	}
+
+	return val;
+}

src/drivers/device/vdev_posix.cpp

@@ -195,7 +195,7 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
 {
 	sem_t sem;
 	int count = 0;
-	int ret;
+	int ret = -1;
 	unsigned int i;
 
 	PX4_DEBUG("Called px4_poll timeout = %d", timeout);
@@ -222,7 +222,7 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
 
 	if (ret >= 0)
 	{
-		if (timeout >= 0)
+		if (timeout > 0)
 		{
 			// Use a work queue task
 			work_s _hpwork;
@@ -234,7 +234,7 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
 			// out of scope
 			hrt_work_cancel(&_hpwork);
         	}
-		else
+		else if (timeout < 0) 
 		{
 			sem_wait(&sem);
 		}
@@ -270,7 +270,7 @@ int px4_fsync(int fd)
 
 int px4_access(const char *pathname, int mode)
 {
-	if (mode == F_OK) {
+	if (mode != F_OK) {
 		errno = EINVAL;
 		return -1;
 	}

src/drivers/frsky_telemetry/frsky_data.c

@@ -178,12 +178,12 @@ void frsky_send_frame1(int uart)
 			roundf(raw.accelerometer_m_s2[2] * 1000.0f));
 
 	frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP,
-			raw.baro_alt_meter);
+			raw.baro_alt_meter[0]);
 	frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP,
-			roundf(frac(raw.baro_alt_meter) * 100.0f));
+			roundf(frac(raw.baro_alt_meter[0]) * 100.0f));
 
 	frsky_send_data(uart, FRSKY_ID_TEMP1,
-			roundf(raw.baro_temp_celcius));
+			roundf(raw.baro_temp_celcius[0]));
 
 	frsky_send_data(uart, FRSKY_ID_VFAS,
 			roundf(battery.voltage_v * 10.0f));

src/drivers/hott/messages.cpp

@@ -148,12 +148,12 @@ build_eam_response(uint8_t *buffer, size_t *size)
 	msg.eam_sensor_id = EAM_SENSOR_ID;
 	msg.sensor_text_id = EAM_SENSOR_TEXT_ID;
 	
-	msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20);
+	msg.temperature1 = (uint8_t)(raw.baro_temp_celcius[0] + 20);
 	msg.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG;
 
 	msg.main_voltage_L = (uint8_t)(battery.voltage_v * 10);
 
-	uint16_t alt = (uint16_t)(raw.baro_alt_meter + 500);
+	uint16_t alt = (uint16_t)(raw.baro_alt_meter[0] + 500);
 	msg.altitude_L = (uint8_t)alt & 0xff;
 	msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
 

src/drivers/l3gd20/l3gd20.cpp

@@ -66,6 +66,7 @@
 #include <drivers/device/spi.h>
 #include <drivers/drv_gyro.h>
 #include <drivers/device/ringbuffer.h>
+#include <drivers/device/integrator.h>
 
 #include <board_config.h>
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
@@ -175,6 +176,7 @@ static const int ERROR = -1;
 
 #define L3GD20_DEFAULT_RATE			760
 #define L3G4200D_DEFAULT_RATE			800
+#define L3GD20_MAX_OUTPUT_RATE			280
 #define L3GD20_DEFAULT_RANGE_DPS		2000
 #define L3GD20_DEFAULT_FILTER_FREQ		30
 #define L3GD20_TEMP_OFFSET_CELSIUS		40
@@ -256,6 +258,8 @@ private:
 	math::LowPassFilter2p	_gyro_filter_y;
 	math::LowPassFilter2p	_gyro_filter_z;
 
+	Integrator		_gyro_int;
+
 	/* true if an L3G4200D is detected */
 	bool	_is_l3g4200d;
 
@@ -427,6 +431,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati
 	_gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
 	_gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
 	_gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
+	_gyro_int(1000000 / L3GD20_MAX_OUTPUT_RATE, true),
 	_is_l3g4200d(false),
 	_rotation(rotation),
 	_checked_next(0)
@@ -1029,13 +1034,21 @@ L3GD20::measure()
 	// apply user specified rotation
 	rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
 
-	report.x = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
-	report.y = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
-	report.z = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
+	float xin = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
+	float yin = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
+	float zin = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
 
-	report.x = _gyro_filter_x.apply(report.x);
-	report.y = _gyro_filter_y.apply(report.y);
-	report.z = _gyro_filter_z.apply(report.z);
+	report.x = _gyro_filter_x.apply(xin);
+	report.y = _gyro_filter_y.apply(yin);
+	report.z = _gyro_filter_z.apply(zin);
+
+	math::Vector<3> gval(xin, yin, zin);
+	math::Vector<3> gval_integrated;
+
+	bool gyro_notify = _gyro_int.put(report.timestamp, gval, gval_integrated, report.integral_dt);
+	report.x_integral = gval_integrated(0);
+	report.y_integral = gval_integrated(1);
+	report.z_integral = gval_integrated(2);
 
 	report.temperature = L3GD20_TEMP_OFFSET_CELSIUS - raw_report.temp;
 
@@ -1044,13 +1057,15 @@ L3GD20::measure()
 
 	_reports->force(&report);
 
-	/* notify anyone waiting for data */
-	poll_notify(POLLIN);
+	if (gyro_notify) {
+		/* notify anyone waiting for data */
+		poll_notify(POLLIN);
 
-	/* publish for subscribers */
-	if (!(_pub_blocked)) {
-		/* publish it */
-		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
+		/* publish for subscribers */
+		if (!(_pub_blocked)) {
+			/* publish it */
+			orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
+		}
 	}
 
 	_read++;

src/drivers/ll40ls/ll40ls.cpp

@@ -113,6 +113,10 @@ LidarLite * get_dev(const bool use_i2c, const int bus) {
  */
 void start(const bool use_i2c, const int bus)
 {
+	if (g_dev_int != nullptr || g_dev_ext != nullptr || g_dev_pwm != nullptr) {
+		errx(1,"driver already started");
+	}
+
 	if (use_i2c) {
 		/* create the driver, attempt expansion bus first */
 		if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) {
@@ -464,7 +468,6 @@ ll40ls_main(int argc, char *argv[])
 
 		/* Start/load the driver. */
 		if (!strcmp(verb, "start")) {
-
 			ll40ls::start(use_i2c, bus);
 		}
 

src/drivers/lsm303d/lsm303d.cpp

@@ -65,6 +65,7 @@
 #include <drivers/drv_accel.h>
 #include <drivers/drv_mag.h>
 #include <drivers/device/ringbuffer.h>
+#include <drivers/device/integrator.h>
 #include <drivers/drv_tone_alarm.h>
 
 #include <board_config.h>
@@ -206,6 +207,7 @@ static const int ERROR = -1;
 #define LSM303D_ACCEL_DEFAULT_RATE			800
 #define LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ	50
 #define LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ	30
+#define LSM303D_ACCEL_MAX_OUTPUT_RATE			280
 
 #define LSM303D_MAG_DEFAULT_RANGE_GA			2
 #define LSM303D_MAG_DEFAULT_RATE			100
@@ -308,6 +310,8 @@ private:
 	math::LowPassFilter2p	_accel_filter_y;
 	math::LowPassFilter2p	_accel_filter_z;
 
+	Integrator		_accel_int;
+
 	enum Rotation		_rotation;
 
 	// values used to
@@ -577,6 +581,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota
 	_accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
 	_accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
 	_accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+	_accel_int(1000000 / LSM303D_ACCEL_MAX_OUTPUT_RATE, true),
 	_rotation(rotation),
 	_constant_accel_count(0),
 	_last_temperature(0),
@@ -1411,6 +1416,13 @@ LSM303D::stop()
 {
 	hrt_cancel(&_accel_call);
 	hrt_cancel(&_mag_call);
+
+	/* reset internal states */
+	memset(_last_accel, 0, sizeof(_last_accel));
+
+	/* discard unread data in the buffers */
+	_accel_reports->flush();
+	_mag_reports->flush();
 }
 
 void
@@ -1575,17 +1587,27 @@ LSM303D::measure()
 	accel_report.y = _accel_filter_y.apply(y_in_new);
 	accel_report.z = _accel_filter_z.apply(z_in_new);
 
+	math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
+	math::Vector<3> aval_integrated;
+
+	bool accel_notify = _accel_int.put(accel_report.timestamp, aval, aval_integrated, accel_report.integral_dt);
+	accel_report.x_integral = aval_integrated(0);
+	accel_report.y_integral = aval_integrated(1);
+	accel_report.z_integral = aval_integrated(2);
+
 	accel_report.scaling = _accel_range_scale;
 	accel_report.range_m_s2 = _accel_range_m_s2;
 
 	_accel_reports->force(&accel_report);
 
 	/* notify anyone waiting for data */
-	poll_notify(POLLIN);
+	if (accel_notify) {
+		poll_notify(POLLIN);
 
-	if (!(_pub_blocked)) {
-		/* publish it */
-		orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
+		if (!(_pub_blocked)) {
+			/* publish it */
+			orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
+		}
 	}
 
 	_accel_read++;
@@ -1841,7 +1863,7 @@ namespace lsm303d
 
 LSM303D	*g_dev;
 
-void	start(bool external_bus, enum Rotation rotation);
+void	start(bool external_bus, enum Rotation rotation, unsigned range);
 void	test();
 void	reset();
 void	info();
@@ -1856,11 +1878,12 @@ void	test_error();
  * up and running or failed to detect the sensor.
  */
 void
-start(bool external_bus, enum Rotation rotation)
+start(bool external_bus, enum Rotation rotation, unsigned range)
 {
 	int fd, fd_mag;
-	if (g_dev != nullptr)
+	if (g_dev != nullptr) {
 		errx(0, "already started");
+	}
 
 	/* create the driver */
         if (external_bus) {
@@ -1884,11 +1907,17 @@ start(bool external_bus, enum Rotation rotation)
 	/* set the poll rate to default, starts automatic data collection */
 	fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY);
 
-	if (fd < 0)
+	if (fd < 0) {
 		goto fail;
+	}
 
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
 		goto fail;
+	}
+
+	if (ioctl(fd, ACCELIOCSRANGE, range) < 0) {
+		goto fail;
+	}
 
 	fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY);
 
@@ -1980,7 +2009,10 @@ test()
 	warnx("mag z: \t%d\traw", (int)m_report.z_raw);
 	warnx("mag range: %8.4f ga", (double)m_report.range_ga);
 
-	/* XXX add poll-rate tests here too */
+	/* reset to default polling */
+	if (ioctl(fd_accel, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+		err(1, "reset to default polling");
+	}
 
         close(fd_accel);
         close(fd_mag);
@@ -2084,9 +2116,10 @@ lsm303d_main(int argc, char *argv[])
 	bool external_bus = false;
 	int ch;
 	enum Rotation rotation = ROTATION_NONE;
+	int accel_range = 8;
 
 	/* jump over start/off/etc and look at options first */
-	while ((ch = getopt(argc, argv, "XR:")) != EOF) {
+	while ((ch = getopt(argc, argv, "XR:a:")) != EOF) {
 		switch (ch) {
 		case 'X':
 			external_bus = true;
@@ -2094,6 +2127,9 @@ lsm303d_main(int argc, char *argv[])
 		case 'R':
 			rotation = (enum Rotation)atoi(optarg);
 			break;
+		case 'a':
+			accel_range = atoi(optarg);
+			break;
 		default:
 			lsm303d::usage();
 			exit(0);
@@ -2107,7 +2143,7 @@ lsm303d_main(int argc, char *argv[])
 
 	 */
 	if (!strcmp(verb, "start"))
-		lsm303d::start(external_bus, rotation);
+		lsm303d::start(external_bus, rotation, accel_range);
 
 	/*
 	 * Test the driver/device.

src/drivers/md25/md25.cpp

@@ -333,7 +333,7 @@ void MD25::update()
 	// check for exit condition every second
 	// note "::poll" is required to distinguish global
 	// poll from member function for driver
-	if (::poll(&_controlPoll, 1, 1000) < 0) return; // poll error
+	if (::poll(&_controlPoll, 1, 1000) < 0) { return; } // poll error
 
 	// if new data, send to motors
 	if (_actuators.updated()) {
@@ -350,7 +350,7 @@ int MD25::probe()
 	int ret = OK;
 
 	// try initial address first, if good, then done
-	if (readData() == OK) return ret;
+	if (readData() == OK) { return ret; }
 
 	// try all other addresses
 	uint8_t testAddress = 0;
@@ -451,9 +451,9 @@ float MD25::_uint8ToNorm(uint8_t value)
 
 uint8_t MD25::_normToUint8(float value)
 {
-	if (value > 1) value = 1;
+	if (value > 1) { value = 1; }
 
-	if (value < -1) value = -1;
+	if (value < -1) { value = -1; }
 
 	// TODO, should go from 0 to 255
 	// possibly should handle this differently
@@ -494,7 +494,7 @@ int md25Test(const char *deviceName, uint8_t bus, uint8_t address)
 			break;
 		}
 
-		if (t > 2.0f) break;
+		if (t > 2.0f) { break; }
 	}
 
 	md25.setMotor1Speed(0);
@@ -514,7 +514,7 @@ int md25Test(const char *deviceName, uint8_t bus, uint8_t address)
 			break;
 		}
 
-		if (t > 2.0f) break;
+		if (t > 2.0f) { break; }
 	}
 
 	md25.setMotor1Speed(0);
@@ -536,7 +536,7 @@ int md25Test(const char *deviceName, uint8_t bus, uint8_t address)
 			break;
 		}
 
-		if (t > 2.0f) break;
+		if (t > 2.0f) { break; }
 	}
 
 	md25.setMotor2Speed(0);
@@ -556,7 +556,7 @@ int md25Test(const char *deviceName, uint8_t bus, uint8_t address)
 			break;
 		}
 
-		if (t > 2.0f) break;
+		if (t > 2.0f) { break; }
 	}
 
 	md25.setMotor2Speed(0);
@@ -592,13 +592,14 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitu
 
 	// sine wave for motor 1
 	md25.resetEncoders();
+
 	while (true) {
 
 		// input
 		uint64_t timestamp = hrt_absolute_time();
-		float t = timestamp/1000000.0f;
+		float t = timestamp / 1000000.0f;
 
-		float input_value = amplitude*sinf(2*M_PI*frequency*t);
+		float input_value = amplitude * sinf(2 * M_PI * frequency * t);
 		md25.setMotor1Speed(input_value);
 
 		// output
@@ -613,11 +614,11 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitu
 
 		// send output message
 		strncpy(debug_msg.key, "md25 out  ", 10);
-		debug_msg.timestamp_ms = 1000*timestamp;
+		debug_msg.timestamp_ms = 1000 * timestamp;
 		debug_msg.value = current_revolution;
 		debug_msg.update();
 
-		if (t > t_final) break;
+		if (t > t_final) { break; }
 
 		// update for next step
 		prev_revolution = current_revolution;
@@ -625,6 +626,7 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitu
 		// sleep
 		usleep(1000000 * dt);
 	}
+
 	md25.setMotor1Speed(0);
 
 	printf("md25 sine complete\n");

src/drivers/md25/md25_main.cpp

@@ -79,8 +79,9 @@ static void usage(const char *reason);
 static void
 usage(const char *reason)
 {
-	if (reason)
+	if (reason) {
 		fprintf(stderr, "%s\n", reason);
+	}
 
 	fprintf(stderr, "usage: md25 {start|stop|read|status|search|test|change_address}\n\n");
 	exit(1);
@@ -111,11 +112,11 @@ int md25_main(int argc, char *argv[])
 
 		thread_should_exit = false;
 		deamon_task = px4_task_spawn_cmd("md25",
-					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 10,
-					 2048,
-					 md25_thread_main,
-					 (const char **)argv);
+						 SCHED_DEFAULT,
+						 SCHED_PRIORITY_MAX - 10,
+						 2048,
+						 md25_thread_main,
+						 (const char **)argv);
 		exit(0);
 	}
 
@@ -206,7 +207,7 @@ int md25_main(int argc, char *argv[])
 
 		exit(0);
 	}
-	
+
 
 	if (!strcmp(argv[1], "search")) {
 		if (argc < 3) {

src/drivers/mpu6000/mpu6000.cpp

@@ -69,6 +69,7 @@
 
 #include <drivers/device/spi.h>
 #include <drivers/device/ringbuffer.h>
+#include <drivers/device/integrator.h>
 #include <drivers/drv_accel.h>
 #include <drivers/drv_gyro.h>
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
@@ -167,10 +168,13 @@
 
 #define MPU6000_ACCEL_DEFAULT_RANGE_G			8
 #define MPU6000_ACCEL_DEFAULT_RATE			1000
+#define MPU6000_ACCEL_MAX_OUTPUT_RATE			280
 #define MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ	30
 
 #define MPU6000_GYRO_DEFAULT_RANGE_G			8
 #define MPU6000_GYRO_DEFAULT_RATE			1000
+/* rates need to be the same between accel and gyro */
+#define MPU6000_GYRO_MAX_OUTPUT_RATE			MPU6000_ACCEL_MAX_OUTPUT_RATE
 #define MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ		30
 
 #define MPU6000_DEFAULT_ONCHIP_FILTER_FREQ		42
@@ -281,6 +285,9 @@ private:
 	math::LowPassFilter2p	_gyro_filter_y;
 	math::LowPassFilter2p	_gyro_filter_z;
 
+	Integrator		_accel_int;
+	Integrator		_gyro_int;
+
 	enum Rotation		_rotation;
 
 	// this is used to support runtime checking of key
@@ -535,6 +542,8 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
 	_gyro_filter_x(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
 	_gyro_filter_y(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
 	_gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
+	_accel_int(1000000 / MPU6000_ACCEL_MAX_OUTPUT_RATE),
+	_gyro_int(1000000 / MPU6000_GYRO_MAX_OUTPUT_RATE, true),
 	_rotation(rotation),
 	_checked_next(0),
 	_in_factory_test(false),
@@ -1525,6 +1534,13 @@ void
 MPU6000::stop()
 {
 	hrt_cancel(&_call);
+
+	/* reset internal states */
+	memset(_last_accel, 0, sizeof(_last_accel));
+
+	/* discard unread data in the buffers */
+	_accel_reports->flush();
+	_gyro_reports->flush();
 }
 
 void
@@ -1759,6 +1775,14 @@ MPU6000::measure()
 	arb.y = _accel_filter_y.apply(y_in_new);
 	arb.z = _accel_filter_z.apply(z_in_new);
 
+	math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
+	math::Vector<3> aval_integrated;
+
+	bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt);
+	arb.x_integral = aval_integrated(0);
+	arb.y_integral = aval_integrated(1);
+	arb.z_integral = aval_integrated(2);
+
 	arb.scaling = _accel_range_scale;
 	arb.range_m_s2 = _accel_range_m_s2;
 
@@ -1786,6 +1810,14 @@ MPU6000::measure()
 	grb.y = _gyro_filter_y.apply(y_gyro_in_new);
 	grb.z = _gyro_filter_z.apply(z_gyro_in_new);
 
+	math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
+	math::Vector<3> gval_integrated;
+
+	bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt);
+	grb.x_integral = gval_integrated(0);
+	grb.y_integral = gval_integrated(1);
+	grb.z_integral = gval_integrated(2);
+
 	grb.scaling = _gyro_range_scale;
 	grb.range_rad_s = _gyro_range_rad_s;
 
@@ -1796,10 +1828,15 @@ MPU6000::measure()
 	_gyro_reports->force(&grb);
 
 	/* notify anyone waiting for data */
-	poll_notify(POLLIN);
-	_gyro->parent_poll_notify();
+	if (accel_notify) {
+		poll_notify(POLLIN);
+	}
 
-	if (!(_pub_blocked)) {
+	if (gyro_notify) {
+		_gyro->parent_poll_notify();
+	}
+
+	if (accel_notify && !(_pub_blocked)) {
 		/* log the time of this report */
 		perf_begin(_controller_latency_perf);
 		perf_begin(_system_latency_perf);
@@ -1807,7 +1844,7 @@ MPU6000::measure()
 		orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
 	}
 
-	if (!(_pub_blocked)) {
+	if (gyro_notify && !(_pub_blocked)) {
 		/* publish it */
 		orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
 	}
@@ -1925,7 +1962,7 @@ namespace mpu6000
 MPU6000	*g_dev_int; // on internal bus
 MPU6000	*g_dev_ext; // on external bus
 
-void	start(bool, enum Rotation);
+void	start(bool, enum Rotation, int range);
 void	stop(bool);
 void	test(bool);
 void	reset(bool);
@@ -1942,7 +1979,7 @@ void	usage();
  * or failed to detect the sensor.
  */
 void
-start(bool external_bus, enum Rotation rotation)
+start(bool external_bus, enum Rotation rotation, int range)
 {
 	int fd;
         MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int;
@@ -1979,6 +2016,9 @@ start(bool external_bus, enum Rotation rotation)
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
 		goto fail;
 
+	if (ioctl(fd, ACCELIOCSRANGE, range) < 0)
+		goto fail;
+
         close(fd);
 
 	exit(0);
@@ -2076,6 +2116,12 @@ test(bool external_bus)
 	warnx("temp:  \t%8.4f\tdeg celsius", (double)a_report.temperature);
 	warnx("temp:  \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw);
 
+	/* reset to default polling */
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+		err(1, "reset to default polling");
+
+	close(fd);
+	close(fd_gyro);
 
 	/* XXX add poll-rate tests here too */
 
@@ -2175,6 +2221,7 @@ usage()
 	warnx("options:");
 	warnx("    -X    (external bus)");
 	warnx("    -R rotation");
+	warnx("    -a accel range (in g)");
 }
 
 } // namespace
@@ -2185,9 +2232,10 @@ mpu6000_main(int argc, char *argv[])
 	bool external_bus = false;
 	int ch;
 	enum Rotation rotation = ROTATION_NONE;
+	int accel_range = 8;
 
 	/* jump over start/off/etc and look at options first */
-	while ((ch = getopt(argc, argv, "XR:")) != EOF) {
+	while ((ch = getopt(argc, argv, "XR:a:")) != EOF) {
 		switch (ch) {
 		case 'X':
 			external_bus = true;
@@ -2195,6 +2243,9 @@ mpu6000_main(int argc, char *argv[])
 		case 'R':
 			rotation = (enum Rotation)atoi(optarg);
 			break;
+		case 'a':
+			accel_range = atoi(optarg);
+			break;
 		default:
 			mpu6000::usage();
 			exit(0);
@@ -2208,7 +2259,7 @@ mpu6000_main(int argc, char *argv[])
 
 	 */
 	if (!strcmp(verb, "start")) {
-		mpu6000::start(external_bus, rotation);
+		mpu6000::start(external_bus, rotation, accel_range);
 	}
 
 	if (!strcmp(verb, "stop")) {

src/drivers/ms5611/ms5611.h

@@ -83,4 +83,4 @@ extern bool crc4(uint16_t *n_prom);
 extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum) __attribute__((weak));
 extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint8_t busnum) __attribute__((weak));
 extern device::Device *MS5611_sim_interface(ms5611::prom_u &prom_buf, uint8_t busnum) __attribute__((weak));
-typedef device::Device* (*MS5611_constructor)(ms5611::prom_u &prom_buf, uint8_t busnum);
+typedef device::Device *(*MS5611_constructor)(ms5611::prom_u &prom_buf, uint8_t busnum);

src/drivers/ms5611/ms5611_i2c.cpp

@@ -31,11 +31,11 @@
  *
  ****************************************************************************/
 
- /**
-  * @file ms5611_i2c.cpp
-  *
-  * I2C interface for MS5611
-  */
+/**
+ * @file ms5611_i2c.cpp
+ *
+ * I2C interface for MS5611
+ */
 
 /* XXX trim includes */
 #include <px4_config.h>
@@ -116,13 +116,13 @@ MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint8_t busnum)
 }
 
 MS5611_I2C::MS5611_I2C(uint8_t bus, ms5611::prom_u &prom) :
-	I2C("MS5611_I2C", 
+	I2C("MS5611_I2C",
 #ifdef __PX4_NUTTX
-nullptr, bus, 0, 400000
+	    nullptr, bus, 0, 400000
 #else
-"/dev/MS5611_I2C", bus, 0
+	    "/dev/MS5611_I2C", bus, 0
 #endif
-),
+	   ),
 	_prom(prom)
 {
 }
@@ -150,6 +150,7 @@ MS5611_I2C::read(device::file_t *handlep, char *data, size_t buflen)
 	/* read the most recent measurement */
 	uint8_t cmd = 0;
 	int ret = transfer(&cmd, 1, &buf[0], 3);
+
 	if (ret == PX4_OK) {
 		/* fetch the raw value */
 		cvt->b[0] = buf[2];
@@ -191,9 +192,9 @@ MS5611_I2C::probe()
 	if ((PX4_OK == _probe_address(MS5611_ADDRESS_1)) ||
 	    (PX4_OK == _probe_address(MS5611_ADDRESS_2))) {
 		/*
-	    	 * Disable retries; we may enable them selectively in some cases,
+		 * Disable retries; we may enable them selectively in some cases,
 		 * but the device gets confused if we retry some of the commands.
-	    	 */
+		 */
 		_retries = 0;
 		return PX4_OK;
 	}
@@ -208,12 +209,14 @@ MS5611_I2C::_probe_address(uint8_t address)
 	set_address(address);
 
 	/* send reset command */
-	if (PX4_OK != _reset())
+	if (PX4_OK != _reset()) {
 		return -EIO;
+	}
 
 	/* read PROM */
-	if (PX4_OK != _read_prom())
+	if (PX4_OK != _read_prom()) {
 		return -EIO;
+	}
 
 	return PX4_OK;
 }
@@ -239,7 +242,7 @@ int
 MS5611_I2C::_measure(unsigned addr)
 {
 	/*
-	 * Disable retries on this command; we can't know whether failure 
+	 * Disable retries on this command; we can't know whether failure
 	 * means the device did or did not see the command.
 	 */
 	_retries = 0;
@@ -267,8 +270,9 @@ MS5611_I2C::_read_prom()
 	for (int i = 0; i < 8; i++) {
 		uint8_t cmd = ADDR_PROM_SETUP + (i * 2);
 
-		if (PX4_OK != transfer(&cmd, 1, &prom_buf[0], 2))
+		if (PX4_OK != transfer(&cmd, 1, &prom_buf[0], 2)) {
 			break;
+		}
 
 		/* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */
 		cvt.b[0] = prom_buf[1];

src/drivers/ms5611/ms5611_nuttx.cpp

@@ -106,7 +106,7 @@ static const int ERROR = -1;
 class MS5611 : public device::CDev
 {
 public:
-	MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char* path);
+	MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path);
 	~MS5611();
 
 	virtual int		init();
@@ -214,7 +214,7 @@ protected:
  */
 extern "C" __EXPORT int ms5611_main(int argc, char *argv[]);
 
-MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char* path) :
+MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path) :
 	CDev("MS5611", path),
 	_interface(interface),
 	_prom(prom_buf.s),
@@ -243,12 +243,14 @@ MS5611::~MS5611()
 	/* make sure we are truly inactive */
 	stop_cycle();
 
-	if (_class_instance != -1)
+	if (_class_instance != -1) {
 		unregister_class_devname(get_devname(), _class_instance);
+	}
 
 	/* free any existing reports */
-	if (_reports != nullptr)
+	if (_reports != nullptr) {
 		delete _reports;
+	}
 
 	// free perf counters
 	perf_free(_sample_perf);
@@ -265,13 +267,14 @@ MS5611::init()
 	int ret;
 
 	ret = CDev::init();
+
 	if (ret != OK) {
 		DEVICE_DEBUG("CDev init failed");
 		goto out;
 	}
 
 	/* allocate basic report buffers */
-	_reports = new ringbuffer::RingBuffer(2, sizeof(baro_report));
+	_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_baro_s));
 
 	if (_reports == nullptr) {
 		DEVICE_DEBUG("can't get memory for reports");
@@ -321,7 +324,7 @@ MS5611::init()
 		ret = OK;
 
 		_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp,
-				&_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT);
+						  &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT);
 
 
 		if (_baro_topic == nullptr) {
@@ -342,8 +345,9 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
 	int ret = 0;
 
 	/* buffer must be large enough */
-	if (count < 1)
+	if (count < 1) {
 		return -ENOSPC;
+	}
 
 	/* if automatic measurement is enabled */
 	if (_measure_ticks > 0) {
@@ -396,8 +400,9 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
 		}
 
 		/* state machine will have generated a report, copy it out */
-		if (_reports->get(brp))
+		if (_reports->get(brp)) {
 			ret = sizeof(*brp);
+		}
 
 	} while (0);
 
@@ -412,20 +417,20 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case SENSORIOCSPOLLRATE: {
 			switch (arg) {
 
-				/* switching to manual polling */
+			/* switching to manual polling */
 			case SENSOR_POLLRATE_MANUAL:
 				stop_cycle();
 				_measure_ticks = 0;
 				return OK;
 
-				/* external signalling not supported */
+			/* external signalling not supported */
 			case SENSOR_POLLRATE_EXTERNAL:
 
-				/* zero would be bad */
+			/* zero would be bad */
 			case 0:
 				return -EINVAL;
 
-				/* set default/max polling rate */
+			/* set default/max polling rate */
 			case SENSOR_POLLRATE_MAX:
 			case SENSOR_POLLRATE_DEFAULT: {
 					/* do we need to start internal polling? */
@@ -435,13 +440,14 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
 					_measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL);
 
 					/* if we need to start the poll state machine, do it */
-					if (want_start)
+					if (want_start) {
 						start_cycle();
+					}
 
 					return OK;
 				}
 
-				/* adjust to a legal polling interval in Hz */
+			/* adjust to a legal polling interval in Hz */
 			default: {
 					/* do we need to start internal polling? */
 					bool want_start = (_measure_ticks == 0);
@@ -450,15 +456,17 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
 					unsigned ticks = USEC2TICK(1000000 / arg);
 
 					/* check against maximum rate */
-					if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL))
+					if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) {
 						return -EINVAL;
+					}
 
 					/* update interval for next measurement */
 					_measure_ticks = ticks;
 
 					/* if we need to start the poll state machine, do it */
-					if (want_start)
+					if (want_start) {
 						start_cycle();
+					}
 
 					return OK;
 				}
@@ -466,24 +474,28 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
 		}
 
 	case SENSORIOCGPOLLRATE:
-		if (_measure_ticks == 0)
+		if (_measure_ticks == 0) {
 			return SENSOR_POLLRATE_MANUAL;
+		}
 
 		return (1000 / _measure_ticks);
 
 	case SENSORIOCSQUEUEDEPTH: {
-		/* lower bound is mandatory, upper bound is a sanity check */
-		if ((arg < 1) || (arg > 100))
-			return -EINVAL;
+			/* lower bound is mandatory, upper bound is a sanity check */
+			if ((arg < 1) || (arg > 100)) {
+				return -EINVAL;
+			}
+
+			irqstate_t flags = irqsave();
+
+			if (!_reports->resize(arg)) {
+				irqrestore(flags);
+				return -ENOMEM;
+			}
 
-		irqstate_t flags = irqsave();
-		if (!_reports->resize(arg)) {
 			irqrestore(flags);
-			return -ENOMEM;
+			return OK;
 		}
-		irqrestore(flags);
-		return OK;
-	}
 
 	case SENSORIOCGQUEUEDEPTH:
 		return _reports->size();
@@ -498,8 +510,9 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case BAROIOCSMSLPRESSURE:
 
 		/* range-check for sanity */
-		if ((arg < 80000) || (arg > 120000))
+		if ((arg < 80000) || (arg > 120000)) {
 			return -EINVAL;
+		}
 
 		_msl_pressure = arg;
 		return OK;
@@ -554,6 +567,7 @@ MS5611::cycle()
 
 		/* perform collection */
 		ret = collect();
+
 		if (ret != OK) {
 			if (ret == -6) {
 				/*
@@ -564,6 +578,7 @@ MS5611::cycle()
 			} else {
 				//DEVICE_LOG("collection error %d", ret);
 			}
+
 			/* issue a reset command to the sensor */
 			_interface->ioctl(IOCTL_RESET, dummy);
 			/* reset the collection state machine and try again - we need
@@ -598,6 +613,7 @@ MS5611::cycle()
 
 	/* measurement phase */
 	ret = measure();
+
 	if (ret != OK) {
 		/* issue a reset command to the sensor */
 		_interface->ioctl(IOCTL_RESET, dummy);
@@ -633,8 +649,10 @@ MS5611::measure()
 	 * Send the command to begin measuring.
 	 */
 	ret = _interface->ioctl(IOCTL_MEASURE, addr);
-	if (OK != ret)
+
+	if (OK != ret) {
 		perf_count(_comms_errors);
+	}
 
 	perf_end(_measure_perf);
 
@@ -652,10 +670,11 @@ MS5611::collect()
 	struct baro_report report;
 	/* this should be fairly close to the end of the conversion, so the best approximation of the time */
 	report.timestamp = hrt_absolute_time();
-        report.error_count = perf_event_count(_comms_errors);
+	report.error_count = perf_event_count(_comms_errors);
 
 	/* read the most recent measurement - read offset/size are hardcoded in the interface */
 	ret = _interface->read(0, (void *)&raw, 0);
+
 	if (ret < 0) {
 		perf_count(_comms_errors);
 		perf_end(_sample_perf);
@@ -883,11 +902,13 @@ crc4(uint16_t *n_prom)
 bool
 start_bus(struct ms5611_bus_option &bus)
 {
-	if (bus.dev != nullptr)
-		errx(1,"bus option already started");
+	if (bus.dev != nullptr) {
+		errx(1, "bus option already started");
+	}
 
 	prom_u prom_buf;
 	device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum);
+
 	if (interface->init() != OK) {
 		delete interface;
 		warnx("no device on bus %u", (unsigned)bus.busid);
@@ -895,6 +916,7 @@ start_bus(struct ms5611_bus_option &bus)
 	}
 
 	bus.dev = new MS5611(interface, prom_buf, bus.devpath);
+
 	if (bus.dev != nullptr && OK != bus.dev->init()) {
 		delete bus.dev;
 		bus.dev = NULL;
@@ -907,6 +929,7 @@ start_bus(struct ms5611_bus_option &bus)
 	if (fd == -1) {
 		errx(1, "can't open baro device");
 	}
+
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
 		close(fd);
 		errx(1, "failed setting default poll rate");
@@ -929,20 +952,23 @@ start(enum MS5611_BUS busid)
 	uint8_t i;
 	bool started = false;
 
-	for (i=0; i<NUM_BUS_OPTIONS; i++) {
+	for (i = 0; i < NUM_BUS_OPTIONS; i++) {
 		if (busid == MS5611_BUS_ALL && bus_options[i].dev != NULL) {
 			// this device is already started
 			continue;
 		}
+
 		if (busid != MS5611_BUS_ALL && bus_options[i].busid != busid) {
 			// not the one that is asked for
 			continue;
 		}
+
 		started |= start_bus(bus_options[i]);
 	}
 
-	if (!started)
+	if (!started) {
 		errx(1, "driver start failed");
+	}
 
 	// one or more drivers started OK
 	exit(0);
@@ -954,13 +980,14 @@ start(enum MS5611_BUS busid)
  */
 struct ms5611_bus_option &find_bus(enum MS5611_BUS busid)
 {
-	for (uint8_t i=0; i<NUM_BUS_OPTIONS; i++) {
+	for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) {
 		if ((busid == MS5611_BUS_ALL ||
 		     busid == bus_options[i].busid) && bus_options[i].dev != NULL) {
 			return bus_options[i];
 		}
 	}
-	errx(1,"bus %u not started", (unsigned)busid);
+
+	errx(1, "bus %u not started", (unsigned)busid);
 }
 
 /**
@@ -979,14 +1006,17 @@ test(enum MS5611_BUS busid)
 	int fd;
 
 	fd = open(bus.devpath, O_RDONLY);
-	if (fd < 0)
+
+	if (fd < 0) {
 		err(1, "open failed (try 'ms5611 start' if the driver is not running)");
+	}
 
 	/* do a simple demand read */
 	sz = read(fd, &report, sizeof(report));
 
-	if (sz != sizeof(report))
+	if (sz != sizeof(report)) {
 		err(1, "immediate read failed");
+	}
 
 	warnx("single read");
 	warnx("pressure:    %10.4f", (double)report.pressure);
@@ -995,12 +1025,14 @@ test(enum MS5611_BUS busid)
 	warnx("time:        %lld", report.timestamp);
 
 	/* set the queue depth to 10 */
-	if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10))
+	if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) {
 		errx(1, "failed to set queue depth");
+	}
 
 	/* start the sensor polling at 2Hz */
-	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
 		errx(1, "failed to set 2Hz poll rate");
+	}
 
 	/* read the sensor 5x and report each value */
 	for (unsigned i = 0; i < 5; i++) {
@@ -1011,14 +1043,16 @@ test(enum MS5611_BUS busid)
 		fds.events = POLLIN;
 		ret = poll(&fds, 1, 2000);
 
-		if (ret != 1)
+		if (ret != 1) {
 			errx(1, "timed out waiting for sensor data");
+		}
 
 		/* now go get it */
 		sz = read(fd, &report, sizeof(report));
 
-		if (sz != sizeof(report))
+		if (sz != sizeof(report)) {
 			err(1, "periodic read failed");
+		}
 
 		warnx("periodic read %u", i);
 		warnx("pressure:    %10.4f", (double)report.pressure);
@@ -1041,14 +1075,18 @@ reset(enum MS5611_BUS busid)
 	int fd;
 
 	fd = open(bus.devpath, O_RDONLY);
-	if (fd < 0)
+
+	if (fd < 0) {
 		err(1, "failed ");
+	}
 
-	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
 		err(1, "driver reset failed");
+	}
 
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
 		err(1, "driver poll restart failed");
+	}
 
 	exit(0);
 }
@@ -1059,13 +1097,15 @@ reset(enum MS5611_BUS busid)
 void
 info()
 {
-	for (uint8_t i=0; i<NUM_BUS_OPTIONS; i++) {
+	for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) {
 		struct ms5611_bus_option &bus = bus_options[i];
+
 		if (bus.dev != nullptr) {
 			warnx("%s", bus.devpath);
 			bus.dev->print_info();
 		}
 	}
+
 	exit(0);
 }
 
@@ -1084,12 +1124,14 @@ calibrate(unsigned altitude, enum MS5611_BUS busid)
 
 	fd = open(bus.devpath, O_RDONLY);
 
-	if (fd < 0)
+	if (fd < 0) {
 		err(1, "open failed (try 'ms5611 start' if the driver is not running)");
+	}
 
 	/* start the sensor polling at max */
-	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX))
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) {
 		errx(1, "failed to set poll rate");
+	}
 
 	/* average a few measurements */
 	pressure = 0.0f;
@@ -1104,14 +1146,16 @@ calibrate(unsigned altitude, enum MS5611_BUS busid)
 		fds.events = POLLIN;
 		ret = poll(&fds, 1, 1000);
 
-		if (ret != 1)
+		if (ret != 1) {
 			errx(1, "timed out waiting for sensor data");
+		}
 
 		/* now go get it */
 		sz = read(fd, &report, sizeof(report));
 
-		if (sz != sizeof(report))
+		if (sz != sizeof(report)) {
 			err(1, "sensor read failed");
+		}
 
 		pressure += report.pressure;
 	}
@@ -1134,8 +1178,9 @@ calibrate(unsigned altitude, enum MS5611_BUS busid)
 	/* save as integer Pa */
 	p1 *= 1000.0f;
 
-	if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK)
+	if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) {
 		err(1, "BAROIOCSMSLPRESSURE");
+	}
 
 	close(fd);
 	exit(0);
@@ -1166,15 +1211,19 @@ ms5611_main(int argc, char *argv[])
 		case 'X':
 			busid = MS5611_BUS_I2C_EXTERNAL;
 			break;
+
 		case 'I':
 			busid = MS5611_BUS_I2C_INTERNAL;
 			break;
+
 		case 'S':
 			busid = MS5611_BUS_SPI_EXTERNAL;
 			break;
+
 		case 's':
 			busid = MS5611_BUS_SPI_INTERNAL;
 			break;
+
 		default:
 			ms5611::usage();
 			exit(0);
@@ -1215,10 +1264,11 @@ ms5611_main(int argc, char *argv[])
 	 * Perform MSL pressure calibration given an altitude in metres
 	 */
 	if (!strcmp(verb, "calibrate")) {
-		if (argc < 2)
+		if (argc < 2) {
 			errx(1, "missing altitude");
+		}
 
-		long altitude = strtol(argv[optind+1], nullptr, 10);
+		long altitude = strtol(argv[optind + 1], nullptr, 10);
 
 		ms5611::calibrate(altitude, busid);
 	}

src/drivers/ms5611/ms5611_posix.cpp

@@ -105,7 +105,7 @@ static const int ERROR = -1;
 class MS5611 : public device::VDev
 {
 public:
-	MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char* path);
+	MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path);
 	~MS5611();
 
 	virtual int		init();
@@ -211,7 +211,7 @@ protected:
  */
 extern "C" __EXPORT int ms5611_main(int argc, char *argv[]);
 
-MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char* path) :
+MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path) :
 	VDev("MS5611", path),
 	_interface(interface),
 	_prom(prom_buf.s),
@@ -240,12 +240,14 @@ MS5611::~MS5611()
 	/* make sure we are truly inactive */
 	stop_cycle();
 
-	if (_class_instance != -1)
+	if (_class_instance != -1) {
 		unregister_class_devname(get_devname(), _class_instance);
+	}
 
 	/* free any existing reports */
-	if (_reports != nullptr)
+	if (_reports != nullptr) {
 		delete _reports;
+	}
 
 	// free perf counters
 	perf_free(_sample_perf);
@@ -263,6 +265,7 @@ MS5611::init()
 	warnx("MS5611::init");
 
 	ret = VDev::init();
+
 	if (ret != OK) {
 		DEVICE_DEBUG("VDev init failed");
 		goto out;
@@ -323,11 +326,12 @@ MS5611::init()
 		ret = OK;
 
 		_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp,
-				&_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT);
+						  &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT);
 
 		if (_baro_topic == nullptr) {
 			warnx("failed to create sensor_baro publication");
 		}
+
 		//warnx("sensor_baro publication %ld", _baro_topic);
 
 	} while (0);
@@ -344,8 +348,9 @@ MS5611::read(device::file_t *handlep, char *buffer, size_t buflen)
 	int ret = 0;
 
 	/* buffer must be large enough */
-	if (count < 1)
+	if (count < 1) {
 		return -ENOSPC;
+	}
 
 	/* if automatic measurement is enabled */
 	if (_measure_ticks > 0) {
@@ -398,8 +403,9 @@ MS5611::read(device::file_t *handlep, char *buffer, size_t buflen)
 		}
 
 		/* state machine will have generated a report, copy it out */
-		if (_reports->get(brp))
+		if (_reports->get(brp)) {
 			ret = sizeof(*brp);
+		}
 
 	} while (0);
 
@@ -414,20 +420,20 @@ MS5611::ioctl(device::file_t *handlep, int cmd, unsigned long arg)
 	case SENSORIOCSPOLLRATE: {
 			switch (arg) {
 
-				/* switching to manual polling */
+			/* switching to manual polling */
 			case SENSOR_POLLRATE_MANUAL:
 				stop_cycle();
 				_measure_ticks = 0;
 				return OK;
 
-				/* external signalling not supported */
+			/* external signalling not supported */
 			case SENSOR_POLLRATE_EXTERNAL:
 
-				/* zero would be bad */
+			/* zero would be bad */
 			case 0:
 				return -EINVAL;
 
-				/* set default/max polling rate */
+			/* set default/max polling rate */
 			case SENSOR_POLLRATE_MAX:
 			case SENSOR_POLLRATE_DEFAULT: {
 					/* do we need to start internal polling? */
@@ -437,13 +443,14 @@ MS5611::ioctl(device::file_t *handlep, int cmd, unsigned long arg)
 					_measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL);
 
 					/* if we need to start the poll state machine, do it */
-					if (want_start)
+					if (want_start) {
 						start_cycle();
+					}
 
 					return OK;
 				}
 
-				/* adjust to a legal polling interval in Hz */
+			/* adjust to a legal polling interval in Hz */
 			default: {
 					/* do we need to start internal polling? */
 					bool want_start = (_measure_ticks == 0);
@@ -452,15 +459,17 @@ MS5611::ioctl(device::file_t *handlep, int cmd, unsigned long arg)
 					unsigned ticks = USEC2TICK(1000000 / arg);
 
 					/* check against maximum rate */
-					if ((unsigned long)ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL))
+					if ((unsigned long)ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) {
 						return -EINVAL;
+					}
 
 					/* update interval for next measurement */
 					_measure_ticks = ticks;
 
 					/* if we need to start the poll state machine, do it */
-					if (want_start)
+					if (want_start) {
 						start_cycle();
+					}
 
 					return OK;
 				}
@@ -468,21 +477,24 @@ MS5611::ioctl(device::file_t *handlep, int cmd, unsigned long arg)
 		}
 
 	case SENSORIOCGPOLLRATE:
-		if (_measure_ticks == 0)
+		if (_measure_ticks == 0) {
 			return SENSOR_POLLRATE_MANUAL;
+		}
 
 		return (1000 / _measure_ticks);
 
 	case SENSORIOCSQUEUEDEPTH: {
-		/* lower bound is mandatory, upper bound is a sanity check */
-		if ((arg < 1) || (arg > 100))
-			return -EINVAL;
+			/* lower bound is mandatory, upper bound is a sanity check */
+			if ((arg < 1) || (arg > 100)) {
+				return -EINVAL;
+			}
 
-		if (!_reports->resize(arg)) {
-			return -ENOMEM;
+			if (!_reports->resize(arg)) {
+				return -ENOMEM;
+			}
+
+			return OK;
 		}
-		return OK;
-	}
 
 	case SENSORIOCGQUEUEDEPTH:
 		return _reports->size();
@@ -497,8 +509,9 @@ MS5611::ioctl(device::file_t *handlep, int cmd, unsigned long arg)
 	case BAROIOCSMSLPRESSURE:
 
 		/* range-check for sanity */
-		if ((arg < 80000) || (arg > 120000))
+		if ((arg < 80000) || (arg > 120000)) {
 			return -EINVAL;
+		}
 
 		_msl_pressure = arg;
 		return OK;
@@ -553,6 +566,7 @@ MS5611::cycle()
 
 		/* perform collection */
 		ret = collect();
+
 		if (ret != OK) {
 			if (ret == -6) {
 				/*
@@ -563,6 +577,7 @@ MS5611::cycle()
 			} else {
 				//DEVICE_LOG("collection error %d", ret);
 			}
+
 			/* issue a reset command to the sensor */
 			_interface->dev_ioctl(IOCTL_RESET, dummy);
 			/* reset the collection state machine and try again */
@@ -594,6 +609,7 @@ MS5611::cycle()
 
 	/* measurement phase */
 	ret = measure();
+
 	if (ret != OK) {
 		//DEVICE_LOG("measure error %d", ret);
 		/* issue a reset command to the sensor */
@@ -630,8 +646,10 @@ MS5611::measure()
 	 * Send the command to begin measuring.
 	 */
 	ret = _interface->dev_ioctl(IOCTL_MEASURE, addr);
-	if (OK != ret)
+
+	if (OK != ret) {
 		perf_count(_comms_errors);
+	}
 
 	perf_end(_measure_perf);
 
@@ -649,10 +667,11 @@ MS5611::collect()
 	struct baro_report report;
 	/* this should be fairly close to the end of the conversion, so the best approximation of the time */
 	report.timestamp = hrt_absolute_time();
-        report.error_count = perf_event_count(_comms_errors);
+	report.error_count = perf_event_count(_comms_errors);
 
 	/* read the most recent measurement - read offset/size are hardcoded in the interface */
 	ret = _interface->dev_read(0, (void *)&raw, 0);
+
 	if (ret < 0) {
 		perf_count(_comms_errors);
 		perf_end(_sample_perf);
@@ -745,8 +764,8 @@ MS5611::collect()
 			if (_baro_topic != (orb_advert_t)(-1)) {
 				/* publish it */
 				orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
-			}
-			else {
+
+			} else {
 				printf("MS5611::collect _baro_topic not initialized\n");
 			}
 		}
@@ -897,6 +916,7 @@ start_bus(struct ms5611_bus_option &bus)
 
 	prom_u prom_buf;
 	device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum);
+
 	if (interface->init() != OK) {
 		delete interface;
 		warnx("no device on bus %u", (unsigned)bus.busid);
@@ -904,13 +924,14 @@ start_bus(struct ms5611_bus_option &bus)
 	}
 
 	bus.dev = new MS5611(interface, prom_buf, bus.devpath);
+
 	if (bus.dev != nullptr && OK != bus.dev->init()) {
 		delete bus.dev;
 		bus.dev = NULL;
 		warnx("bus init failed %p", bus.dev);
 		return false;
 	}
-			
+
 	int fd = px4_open(bus.devpath, O_RDONLY);
 
 	/* set the poll rate to default, starts automatic data collection */
@@ -918,6 +939,7 @@ start_bus(struct ms5611_bus_option &bus)
 		warnx("can't open baro device");
 		return false;
 	}
+
 	if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
 		px4_close(fd);
 		warnx("failed setting default poll rate");
@@ -941,15 +963,17 @@ start(enum MS5611_BUS busid)
 	uint8_t i;
 	bool started = false;
 
-	for (i=0; i<NUM_BUS_OPTIONS; i++) {
+	for (i = 0; i < NUM_BUS_OPTIONS; i++) {
 		if (busid == MS5611_BUS_ALL && bus_options[i].dev != NULL) {
 			// this device is already started
 			continue;
 		}
+
 		if (busid != MS5611_BUS_ALL && bus_options[i].busid != busid) {
 			// not the one that is asked for
 			continue;
 		}
+
 		started |= start_bus(bus_options[i]);
 	}
 
@@ -968,15 +992,15 @@ start(enum MS5611_BUS busid)
  */
 bool find_bus(enum MS5611_BUS busid, ms5611_bus_option &bus)
 {
-	for (uint8_t i=0; i<NUM_BUS_OPTIONS; i++) {
+	for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) {
 		if ((busid == MS5611_BUS_ALL ||
 		     busid == bus_options[i].busid) && bus_options[i].dev != NULL) {
 			bus = bus_options[i];
-                        return true;
+			return true;
 		}
 	}
 
-        PX4_WARN("bus %u not started", (unsigned)busid);
+	PX4_WARN("bus %u not started", (unsigned)busid);
 	return false;
 }
 
@@ -989,17 +1013,21 @@ int
 test(enum MS5611_BUS busid)
 {
 	struct ms5611_bus_option bus;
+
 	if (!find_bus(busid, bus)) {
 		return 1;
 	}
 
 	struct baro_report report;
+
 	ssize_t sz;
+
 	int ret;
 
 	int fd;
 
 	fd = px4_open(bus.devpath, O_RDONLY);
+
 	if (fd < 0) {
 		warn("open failed (try 'ms5611 start' if the driver is not running)");
 		return 1;
@@ -1071,6 +1099,7 @@ int
 reset(enum MS5611_BUS busid)
 {
 	struct ms5611_bus_option bus;
+
 	if (!find_bus(busid, bus)) {
 		return 1;
 	}
@@ -1078,6 +1107,7 @@ reset(enum MS5611_BUS busid)
 	int fd;
 
 	fd = open(bus.devpath, O_RDONLY);
+
 	if (fd < 0) {
 		warn("failed ");
 		return 1;
@@ -1092,6 +1122,7 @@ reset(enum MS5611_BUS busid)
 		warn("driver poll restart failed");
 		return 1;
 	}
+
 	return 0;
 }
 
@@ -1101,13 +1132,15 @@ reset(enum MS5611_BUS busid)
 int
 info()
 {
-	for (uint8_t i=0; i<NUM_BUS_OPTIONS; i++) {
+	for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) {
 		struct ms5611_bus_option &bus = bus_options[i];
+
 		if (bus.dev != nullptr) {
 			warnx("%s", bus.devpath);
 			bus.dev->print_info();
 		}
 	}
+
 	return 0;
 }
 
@@ -1118,12 +1151,15 @@ int
 calibrate(unsigned altitude, enum MS5611_BUS busid)
 {
 	struct ms5611_bus_option bus;
+
 	if (!find_bus(busid, bus)) {
 		return 1;
 	}
 
 	struct baro_report report;
+
 	float	pressure;
+
 	float	p1;
 
 	int fd;
@@ -1224,18 +1260,23 @@ ms5611_main(int argc, char *argv[])
 	/* jump over start/off/etc and look at options first */
 	int myoptind = 1;
 	const char *myoptarg = NULL;
+
 	while ((ch = px4_getopt(argc, argv, "XIS", &myoptind, &myoptarg)) != EOF) {
 		printf("ch = %d\n", ch);
+
 		switch (ch) {
 		case 'X':
 			busid = MS5611_BUS_I2C_EXTERNAL;
 			break;
+
 		case 'I':
 			busid = MS5611_BUS_I2C_INTERNAL;
 			break;
+
 		case 'S':
 			busid = MS5611_BUS_SIM_EXTERNAL;
 			break;
+
 		default:
 			ms5611::usage();
 			return 1;
@@ -1247,42 +1288,48 @@ ms5611_main(int argc, char *argv[])
 	/*
 	 * Start/load the driver.
 	 */
-	if (!strcmp(verb, "start"))
+	if (!strcmp(verb, "start")) {
 		ret = ms5611::start(busid);
+	}
 
 	/*
 	 * Test the driver/device.
 	 */
-	else if (!strcmp(verb, "test"))
+	else if (!strcmp(verb, "test")) {
 		ret = ms5611::test(busid);
+	}
 
 	/*
 	 * Reset the driver.
 	 */
-	else if (!strcmp(verb, "reset"))
+	else if (!strcmp(verb, "reset")) {
 		ret = ms5611::reset(busid);
+	}
 
 	/*
 	 * Print driver information.
 	 */
-	else if (!strcmp(verb, "info"))
+	else if (!strcmp(verb, "info")) {
 		ret = ms5611::info();
+	}
 
 	/*
 	 * Perform MSL pressure calibration given an altitude in metres
 	 */
 	else if (!strcmp(verb, "calibrate")) {
-               if (argc < 2)
-                       PX4_WARN("missing altitude");
+		if (argc < 2) {
+			PX4_WARN("missing altitude");
+		}
 
-		long altitude = strtol(argv[optind+1], nullptr, 10);
+		long altitude = strtol(argv[optind + 1], nullptr, 10);
 
 		ret = ms5611::calibrate(altitude, busid);
-	}
-	else {
+
+	} else {
 		ms5611::usage();
 		warnx("unrecognised command, try 'start', 'test', 'reset' or 'info'");
 		return 1;
 	}
+
 	return ret;
 }

src/drivers/ms5611/ms5611_sim.cpp

@@ -31,11 +31,11 @@
  *
  ****************************************************************************/
 
- /**
-  * @file ms5611_i2c.cpp
-  *
-  * SIM interface for MS5611
-  */
+/**
+ * @file ms5611_i2c.cpp
+ *
+ * SIM interface for MS5611
+ */
 
 /* XXX trim includes */
 #include <px4_config.h>
@@ -127,6 +127,7 @@ MS5611_SIM::dev_read(unsigned offset, void *data, unsigned count)
 	/* read the most recent measurement */
 	uint8_t cmd = 0;
 	int ret = transfer(&cmd, 1, &buf[0], 3);
+
 	if (ret == PX4_OK) {
 		/* fetch the raw value */
 		cvt->b[0] = buf[2];
@@ -178,7 +179,7 @@ int
 MS5611_SIM::_measure(unsigned addr)
 {
 	/*
-	 * Disable retries on this command; we can't know whether failure 
+	 * Disable retries on this command; we can't know whether failure
 	 * means the device did or did not see the command.
 	 */
 	_retries = 0;
@@ -197,7 +198,7 @@ MS5611_SIM::_read_prom()
 
 int
 MS5611_SIM::transfer(const uint8_t *send, unsigned send_len,
-				 uint8_t *recv, unsigned recv_len)
+		     uint8_t *recv, unsigned recv_len)
 {
 	// TODO add Simulation data connection so calls retrieve
 	// data from the simulator

src/drivers/ms5611/ms5611_spi.cpp

@@ -31,11 +31,11 @@
  *
  ****************************************************************************/
 
- /**
-  * @file ms5611_spi.cpp
-  *
-  * SPI interface for MS5611
-  */
+/**
+ * @file ms5611_spi.cpp
+ *
+ * SPI interface for MS5611
+ */
 
 /* XXX trim includes */
 #include <px4_config.h>
@@ -117,6 +117,7 @@ device::Device *
 MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum)
 {
 #ifdef PX4_SPI_BUS_EXT
+
 	if (busnum == PX4_SPI_BUS_EXT) {
 #ifdef PX4_SPIDEV_EXT_BARO
 		return new MS5611_SPI(busnum, (spi_dev_e)PX4_SPIDEV_EXT_BARO, prom_buf);
@@ -124,12 +125,13 @@ MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum)
 		return nullptr;
 #endif
 	}
+
 #endif
 	return new MS5611_SPI(busnum, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf);
 }
 
 MS5611_SPI::MS5611_SPI(uint8_t bus, spi_dev_e device, ms5611::prom_u &prom_buf) :
-	SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz */),
+	SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 11 * 1000 * 1000 /* will be rounded to 10.4 MHz */),
 	_prom(prom_buf)
 {
 }
@@ -144,6 +146,7 @@ MS5611_SPI::init()
 	int ret;
 
 	ret = SPI::init();
+
 	if (ret != OK) {
 		DEVICE_DEBUG("SPI init failed");
 		goto out;
@@ -151,6 +154,7 @@ MS5611_SPI::init()
 
 	/* send reset command */
 	ret = _reset();
+
 	if (ret != OK) {
 		DEVICE_DEBUG("reset failed");
 		goto out;
@@ -158,6 +162,7 @@ MS5611_SPI::init()
 
 	/* read PROM */
 	ret = _read_prom();
+
 	if (ret != OK) {
 		DEVICE_DEBUG("prom readout failed");
 		goto out;
@@ -214,6 +219,7 @@ MS5611_SPI::ioctl(unsigned operation, unsigned &arg)
 		errno = ret;
 		return -1;
 	}
+
 	return 0;
 }
 
@@ -244,25 +250,32 @@ MS5611_SPI::_read_prom()
 	usleep(3000);
 
 	/* read and convert PROM words */
-        bool all_zero = true;
+	bool all_zero = true;
+
 	for (int i = 0; i < 8; i++) {
 		uint8_t cmd = (ADDR_PROM_SETUP + (i * 2));
 		_prom.c[i] = _reg16(cmd);
-                if (_prom.c[i] != 0)
+
+		if (_prom.c[i] != 0) {
 			all_zero = false;
-                //DEVICE_DEBUG("prom[%u]=0x%x", (unsigned)i, (unsigned)_prom.c[i]);
+		}
+
+		//DEVICE_DEBUG("prom[%u]=0x%x", (unsigned)i, (unsigned)_prom.c[i]);
 	}
 
 	/* calculate CRC and return success/failure accordingly */
 	int ret = ms5611::crc4(&_prom.c[0]) ? OK : -EIO;
-        if (ret != OK) {
+
+	if (ret != OK) {
 		DEVICE_DEBUG("crc failed");
-        }
-        if (all_zero) {
+	}
+
+	if (all_zero) {
 		DEVICE_DEBUG("prom all zero");
 		ret = -EIO;
-        }
-        return ret;
+	}
+
+	return ret;
 }
 
 uint16_t

src/drivers/pca8574/pca8574.cpp

@@ -188,6 +188,7 @@ PCA8574::ioctl(struct file *filp, int cmd, unsigned long arg)
 				if (_values_out) {
 					_mode = IOX_MODE_ON;
 				}
+
 				send_led_values();
 			}
 
@@ -277,15 +278,18 @@ PCA8574::led()
 
 		_update_out = true;
 		_should_run = true;
+
 	} else if (_mode == IOX_MODE_OFF) {
 		_update_out = true;
 		_should_run = false;
+
 	} else {
 
 		// Any of the normal modes
 		if (_blinking > 0) {
 			/* we need to be running to blink */
 			_should_run = true;
+
 		} else {
 			_should_run = false;
 		}
@@ -304,6 +308,7 @@ PCA8574::led()
 			msg |= ((_blink_phase) ? _blinking : 0);
 
 			_blink_phase = !_blink_phase;
+
 		} else {
 			msg = _values_out;
 		}
@@ -513,6 +518,7 @@ pca8574_main(int argc, char *argv[])
 			printf(".");
 			fflush(stdout);
 		}
+
 		printf("\n");
 		fflush(stdout);
 
@@ -520,6 +526,7 @@ pca8574_main(int argc, char *argv[])
 			delete g_pca8574;
 			g_pca8574 = nullptr;
 			exit(0);
+
 		} else {
 			warnx("stop failed.");
 			exit(1);
@@ -542,9 +549,11 @@ pca8574_main(int argc, char *argv[])
 
 		if (channel < 8) {
 			ret = ioctl(fd, (IOX_SET_VALUE + channel), val);
+
 		} else {
 			ret = -1;
 		}
+
 		close(fd);
 		exit(ret);
 	}

src/drivers/pca9685/pca9685.cpp

@@ -117,7 +117,7 @@ static const int ERROR = -1;
 class PCA9685 : public device::I2C
 {
 public:
-	PCA9685(int bus=PCA9685_BUS, uint8_t address=ADDR);
+	PCA9685(int bus = PCA9685_BUS, uint8_t address = ADDR);
 	virtual ~PCA9685();
 
 
@@ -192,7 +192,7 @@ PCA9685::PCA9685(int bus, uint8_t address) :
 	I2C("pca9685", PCA9685_DEVICE_PATH, bus, address, 100000),
 	_mode(IOX_MODE_OFF),
 	_running(false),
-	_i2cpwm_interval(SEC2TICK(1.0f/60.0f)),
+	_i2cpwm_interval(SEC2TICK(1.0f / 60.0f)),
 	_should_run(false),
 	_comms_errors(perf_alloc(PC_COUNT, "actuator_controls_2_comms_errors")),
 	_actuator_controls_sub(-1),
@@ -213,11 +213,13 @@ PCA9685::init()
 {
 	int ret;
 	ret = I2C::init();
+
 	if (ret != OK) {
 		return ret;
 	}
 
 	ret = reset();
+
 	if (ret != OK) {
 		return ret;
 	}
@@ -231,6 +233,7 @@ int
 PCA9685::ioctl(struct file *filp, int cmd, unsigned long arg)
 {
 	int ret = -EINVAL;
+
 	switch (cmd) {
 
 	case IOX_SET_MODE:
@@ -241,9 +244,11 @@ PCA9685::ioctl(struct file *filp, int cmd, unsigned long arg)
 			case IOX_MODE_OFF:
 				warnx("shutting down");
 				break;
+
 			case IOX_MODE_ON:
 				warnx("starting");
 				break;
+
 			case IOX_MODE_TEST_OUT:
 				warnx("test starting");
 				break;
@@ -280,6 +285,7 @@ PCA9685::info()
 
 	if (is_running()) {
 		warnx("Driver is running, mode: %u", _mode);
+
 	} else {
 		warnx("Driver started but not running");
 	}
@@ -304,8 +310,10 @@ PCA9685::i2cpwm()
 	if (_mode == IOX_MODE_TEST_OUT) {
 		setPin(0, PCA9685_PWMCENTER);
 		_should_run = true;
+
 	} else if (_mode == IOX_MODE_OFF) {
 		_should_run = false;
+
 	} else {
 		if (!_mode_on_initialized) {
 			/* Subscribe to actuator control 2 (payload group for gimbal) */
@@ -319,25 +327,29 @@ PCA9685::i2cpwm()
 		/* Read the servo setpoints from the actuator control topics (gimbal) */
 		bool updated;
 		orb_check(_actuator_controls_sub, &updated);
+
 		if (updated) {
 			orb_copy(ORB_ID(actuator_controls_2), _actuator_controls_sub, &_actuator_controls);
+
 			for (int i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
 				/* Scale the controls to PWM, first multiply by pi to get rad,
 				 * the control[i] values are on the range -1 ... 1 */
 				uint16_t new_value = PCA9685_PWMCENTER +
-					(_actuator_controls.control[i] * M_PI_F * PCA9685_SCALE);
+						     (_actuator_controls.control[i] * M_PI_F * PCA9685_SCALE);
 				DEVICE_DEBUG("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value,
-						(double)_actuator_controls.control[i]);
+					     (double)_actuator_controls.control[i]);
+
 				if (new_value != _current_values[i] &&
-						isfinite(new_value) &&
-						new_value >= PCA9685_PWMMIN &&
-						new_value <= PCA9685_PWMMAX) {
+				    isfinite(new_value) &&
+				    new_value >= PCA9685_PWMMIN &&
+				    new_value <= PCA9685_PWMMAX) {
 					/* This value was updated, send the command to adjust the PWM value */
 					setPin(i, new_value);
 					_current_values[i] = new_value;
 				}
 			}
 		}
+
 		_should_run = true;
 	}
 
@@ -381,23 +393,29 @@ PCA9685::setPin(uint8_t num, uint16_t val, bool invert)
 	if (val > 4095) {
 		val = 4095;
 	}
+
 	if (invert) {
 		if (val == 0) {
 			// Special value for signal fully on.
 			return setPWM(num, 4096, 0);
+
 		} else if (val == 4095) {
 			// Special value for signal fully off.
 			return setPWM(num, 0, 4096);
+
 		} else {
-			return setPWM(num, 0, 4095-val);
+			return setPWM(num, 0, 4095 - val);
 		}
+
 	} else {
 		if (val == 4095) {
 			// Special value for signal fully on.
 			return setPWM(num, 4096, 0);
+
 		} else if (val == 0) {
 			// Special value for signal fully off.
 			return setPWM(num, 0, 4096);
+
 		} else {
 			return setPWM(num, 0, val);
 		}
@@ -419,20 +437,27 @@ PCA9685::setPWMFreq(float freq)
 	uint8_t prescale = uint8_t(prescaleval + 0.5f); //implicit floor()
 	uint8_t oldmode;
 	ret = read8(PCA9685_MODE1, oldmode);
+
 	if (ret != OK) {
 		return ret;
 	}
-	uint8_t newmode = (oldmode&0x7F) | 0x10; // sleep
+
+	uint8_t newmode = (oldmode & 0x7F) | 0x10; // sleep
 
 	ret = write8(PCA9685_MODE1, newmode); // go to sleep
+
 	if (ret != OK) {
 		return ret;
 	}
+
 	ret = write8(PCA9685_PRESCALE, prescale); // set the prescaler
+
 	if (ret != OK) {
 		return ret;
 	}
+
 	ret = write8(PCA9685_MODE1, oldmode);
+
 	if (ret != OK) {
 		return ret;
 	}
@@ -440,6 +465,7 @@ PCA9685::setPWMFreq(float freq)
 	usleep(5000); //5ms delay (from arduino driver)
 
 	ret = write8(PCA9685_MODE1, oldmode | 0xa1);  //  This sets the MODE1 register to turn on auto increment.
+
 	if (ret != OK) {
 		return ret;
 	}
@@ -455,12 +481,14 @@ PCA9685::read8(uint8_t addr, uint8_t &value)
 
 	/* send addr */
 	ret = transfer(&addr, sizeof(addr), nullptr, 0);
+
 	if (ret != OK) {
 		goto fail_read;
 	}
 
 	/* get value */
 	ret = transfer(nullptr, 0, &value, 1);
+
 	if (ret != OK) {
 		goto fail_read;
 	}
@@ -474,23 +502,27 @@ fail_read:
 	return ret;
 }
 
-int PCA9685::reset(void) {
+int PCA9685::reset(void)
+{
 	warnx("resetting");
 	return write8(PCA9685_MODE1, 0x0);
 }
 
 /* Wrapper to wite a byte to addr */
 int
-PCA9685::write8(uint8_t addr, uint8_t value) {
+PCA9685::write8(uint8_t addr, uint8_t value)
+{
 	int ret = OK;
 	_msg[0] = addr;
 	_msg[1] = value;
 	/* send addr and value */
 	ret = transfer(_msg, 2, nullptr, 0);
+
 	if (ret != OK) {
 		perf_count(_comms_errors);
 		DEVICE_LOG("i2c::transfer returned %d", ret);
 	}
+
 	return ret;
 }
 
@@ -571,10 +603,13 @@ pca9685_main(int argc, char *argv[])
 				errx(1, "init failed");
 			}
 		}
+
 		fd = open(PCA9685_DEVICE_PATH, 0);
+
 		if (fd == -1) {
 			errx(1, "Unable to open " PCA9685_DEVICE_PATH);
 		}
+
 		ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_ON);
 		close(fd);
 
@@ -632,14 +667,16 @@ pca9685_main(int argc, char *argv[])
 			printf(".");
 			fflush(stdout);
 		}
+
 		printf("\n");
 		fflush(stdout);
 
 		if (!g_pca9685->is_running()) {
 			delete g_pca9685;
-			g_pca9685= nullptr;
+			g_pca9685 = nullptr;
 			warnx("stopped, exiting");
 			exit(0);
+
 		} else {
 			warnx("stop failed.");
 			exit(1);

src/drivers/px4fmu/fmu.cpp

@@ -197,8 +197,8 @@ private:
 	int		gpio_ioctl(file *filp, int cmd, unsigned long arg);
 
 	/* do not allow to copy due to ptr data members */
-	PX4FMU(const PX4FMU&);
-	PX4FMU operator=(const PX4FMU&);
+	PX4FMU(const PX4FMU &);
+	PX4FMU operator=(const PX4FMU &);
 };
 
 const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
@@ -273,7 +273,7 @@ PX4FMU::PX4FMU() :
 	_mixers(nullptr),
 	_groups_required(0),
 	_groups_subscribed(0),
-	_control_subs{-1},
+	_control_subs{ -1},
 	_poll_fds_num(0),
 	_failsafe_pwm{0},
 	_disarmed_pwm{0},
@@ -335,8 +335,9 @@ PX4FMU::init()
 	/* do regular cdev init */
 	ret = CDev::init();
 
-	if (ret != OK)
+	if (ret != OK) {
 		return ret;
+	}
 
 	/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
 	_class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH);
@@ -352,11 +353,11 @@ PX4FMU::init()
 
 	/* start the IO interface task */
 	_task = px4_task_spawn_cmd("fmuservo",
-			       SCHED_DEFAULT,
-			       SCHED_PRIORITY_DEFAULT,
-			       1200,
-			       (main_t)&PX4FMU::task_main_trampoline,
-			       nullptr);
+				   SCHED_DEFAULT,
+				   SCHED_PRIORITY_DEFAULT,
+				   1200,
+				   (main_t)&PX4FMU::task_main_trampoline,
+				   nullptr);
 
 	if (_task < 0) {
 		DEVICE_DEBUG("task start failed: %d", errno);
@@ -426,6 +427,7 @@ PX4FMU::set_mode(Mode mode)
 		break;
 
 #ifdef CONFIG_ARCH_BOARD_AEROCORE
+
 	case MODE_8PWM: // AeroCore PWMs as 8 PWM outs
 		DEVICE_DEBUG("MODE_8PWM");
 		/* default output rates */
@@ -470,8 +472,9 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
 			// get the channel mask for this rate group
 			uint32_t mask = up_pwm_servo_get_rate_group(group);
 
-			if (mask == 0)
+			if (mask == 0) {
 				continue;
+			}
 
 			// all channels in the group must be either default or alt-rate
 			uint32_t alt = rate_map & mask;
@@ -534,11 +537,13 @@ PX4FMU::subscribe()
 	uint32_t sub_groups = _groups_required & ~_groups_subscribed;
 	uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
 	_poll_fds_num = 0;
+
 	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 		if (sub_groups & (1 << i)) {
 			DEVICE_DEBUG("subscribe to actuator_controls_%d", i);
 			_control_subs[i] = orb_subscribe(_control_topics[i]);
 		}
+
 		if (unsub_groups & (1 << i)) {
 			DEVICE_DEBUG("unsubscribe from actuator_controls_%d", i);
 			::close(_control_subs[i]);
@@ -574,7 +579,8 @@ PX4FMU::update_pwm_rev_mask()
 }
 
 void
-PX4FMU::publish_pwm_outputs(uint16_t *values, size_t numvalues) {
+PX4FMU::publish_pwm_outputs(uint16_t *values, size_t numvalues)
+{
 	actuator_outputs_s outputs;
 	outputs.noutputs = numvalues;
 	outputs.timestamp = hrt_absolute_time();
@@ -586,6 +592,7 @@ PX4FMU::publish_pwm_outputs(uint16_t *values, size_t numvalues) {
 	if (_outputs_pub == nullptr) {
 		int instance = -1;
 		_outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &outputs, &instance, ORB_PRIO_DEFAULT);
+
 	} else {
 		orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs);
 	}
@@ -647,6 +654,7 @@ PX4FMU::task_main()
 			}
 
 			DEVICE_DEBUG("adjusted actuator update interval to %ums", update_rate_in_ms);
+
 			for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 				if (_control_subs[i] > 0) {
 					orb_set_interval(_control_subs[i], update_rate_in_ms);
@@ -674,11 +682,13 @@ PX4FMU::task_main()
 
 			/* get controls for required topics */
 			unsigned poll_id = 0;
+
 			for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 				if (_control_subs[i] > 0) {
 					if (_poll_fds[poll_id].revents & POLLIN) {
 						orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
 					}
+
 					poll_id++;
 				}
 			}
@@ -704,6 +714,7 @@ PX4FMU::task_main()
 				case MODE_8PWM:
 					num_outputs = 8;
 					break;
+
 				default:
 					num_outputs = 0;
 					break;
@@ -723,7 +734,8 @@ PX4FMU::task_main()
 				uint16_t pwm_limited[_max_actuators];
 
 				/* the PWM limit call takes care of out of band errors, NaN and constrains */
-				pwm_limit_calc(_servo_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, outputs, pwm_limited, &_pwm_limit);
+				pwm_limit_calc(_servo_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm,
+					       outputs, pwm_limited, &_pwm_limit);
 
 				/* output to the servos */
 				for (size_t i = 0; i < num_outputs; i++) {
@@ -758,6 +770,7 @@ PX4FMU::task_main()
 		}
 
 		orb_check(_param_sub, &updated);
+
 		if (updated) {
 			parameter_update_s pupdate;
 			orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate);
@@ -809,6 +822,7 @@ PX4FMU::task_main()
 			_control_subs[i] = -1;
 		}
 	}
+
 	::close(_armed_sub);
 	::close(_param_sub);
 
@@ -837,6 +851,7 @@ PX4FMU::control_callback(uintptr_t handle,
 	/* limit control input */
 	if (input > 1.0f) {
 		input = 1.0f;
+
 	} else if (input < -1.0f) {
 		input = -1.0f;
 	}
@@ -844,7 +859,7 @@ PX4FMU::control_callback(uintptr_t handle,
 	/* motor spinup phase - lock throttle to zero */
 	if (_pwm_limit.state == PWM_LIMIT_STATE_RAMP) {
 		if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
-			control_index == actuator_controls_s::INDEX_THROTTLE) {
+		    control_index == actuator_controls_s::INDEX_THROTTLE) {
 			/* limit the throttle output to zero during motor spinup,
 			 * as the motors cannot follow any demand yet
 			 */
@@ -855,7 +870,7 @@ PX4FMU::control_callback(uintptr_t handle,
 	/* throttle not arming - mark throttle input as invalid */
 	if (arm_nothrottle()) {
 		if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
-			control_index == actuator_controls_s::INDEX_THROTTLE) {
+		    control_index == actuator_controls_s::INDEX_THROTTLE) {
 			/* set the throttle to an invalid value */
 			input = NAN_VALUE;
 		}
@@ -973,8 +988,9 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 			_num_failsafe_set = 0;
 
 			for (unsigned i = 0; i < _max_actuators; i++) {
-				if (_failsafe_pwm[i] > 0)
+				if (_failsafe_pwm[i] > 0) {
 					_num_failsafe_set++;
+				}
 			}
 
 			break;
@@ -1021,8 +1037,9 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 			_num_disarmed_set = 0;
 
 			for (unsigned i = 0; i < _max_actuators; i++) {
-				if (_disarmed_pwm[i] > 0)
+				if (_disarmed_pwm[i] > 0) {
 					_num_disarmed_set++;
+				}
 			}
 
 			break;
@@ -1116,12 +1133,14 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		}
 
 #ifdef CONFIG_ARCH_BOARD_AEROCORE
+
 	case PWM_SERVO_SET(7):
 	case PWM_SERVO_SET(6):
 		if (_mode < MODE_8PWM) {
 			ret = -EINVAL;
 			break;
 		}
+
 #endif
 
 	case PWM_SERVO_SET(5):
@@ -1152,12 +1171,14 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		break;
 
 #ifdef CONFIG_ARCH_BOARD_AEROCORE
+
 	case PWM_SERVO_GET(7):
 	case PWM_SERVO_GET(6):
 		if (_mode < MODE_8PWM) {
 			ret = -EINVAL;
 			break;
 		}
+
 #endif
 
 	case PWM_SERVO_GET(5):
@@ -1198,6 +1219,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 	case MIXERIOCGETOUTPUTCOUNT:
 		switch (_mode) {
 #ifdef CONFIG_ARCH_BOARD_AEROCORE
+
 		case MODE_8PWM:
 			*(unsigned *)arg = 8;
 			break;
@@ -1223,43 +1245,46 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		break;
 
 	case PWM_SERVO_SET_COUNT: {
-		/* change the number of outputs that are enabled for
-		 * PWM. This is used to change the split between GPIO
-		 * and PWM under control of the flight config
-		 * parameters. Note that this does not allow for
-		 * changing a set of pins to be used for serial on
-		 * FMUv1
-		 */
-		switch (arg) {
-		case 0:
-			set_mode(MODE_NONE);
-			break;
+			/* change the number of outputs that are enabled for
+			 * PWM. This is used to change the split between GPIO
+			 * and PWM under control of the flight config
+			 * parameters. Note that this does not allow for
+			 * changing a set of pins to be used for serial on
+			 * FMUv1
+			 */
+			switch (arg) {
+			case 0:
+				set_mode(MODE_NONE);
+				break;
 
-		case 2:
-			set_mode(MODE_2PWM);
-			break;
+			case 2:
+				set_mode(MODE_2PWM);
+				break;
 
-		case 4:
-			set_mode(MODE_4PWM);
-			break;
+			case 4:
+				set_mode(MODE_4PWM);
+				break;
 
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
-		case 6:
-			set_mode(MODE_6PWM);
-			break;
+
+			case 6:
+				set_mode(MODE_6PWM);
+				break;
 #endif
 #if defined(CONFIG_ARCH_BOARD_AEROCORE)
-		case 8:
-			set_mode(MODE_8PWM);
-			break;
+
+			case 8:
+				set_mode(MODE_8PWM);
+				break;
 #endif
 
-		default:
-			ret = -EINVAL;
+			default:
+				ret = -EINVAL;
+				break;
+			}
+
 			break;
 		}
-		break;
-	}
 
 	case MIXERIOCRESET:
 		if (_mixers != nullptr) {
@@ -1297,8 +1322,9 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 			const char *buf = (const char *)arg;
 			unsigned buflen = strnlen(buf, 1024);
 
-			if (_mixers == nullptr)
+			if (_mixers == nullptr) {
 				_mixers = new MixerGroup(control_callback, (uintptr_t)_controls);
+			}
 
 			if (_mixers == nullptr) {
 				_groups_required = 0;
@@ -1314,6 +1340,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 					_mixers = nullptr;
 					_groups_required = 0;
 					ret = -EINVAL;
+
 				} else {
 
 					_mixers->groups_required(_groups_required);
@@ -1344,15 +1371,19 @@ PX4FMU::write(file *filp, const char *buffer, size_t len)
 	uint16_t values[6];
 
 #ifdef CONFIG_ARCH_BOARD_AEROCORE
+
 	if (count > 8) {
 		// we have at most 8 outputs
 		count = 8;
 	}
+
 #else
+
 	if (count > 6) {
 		// we have at most 6 outputs
 		count = 6;
 	}
+
 #endif
 
 	// allow for misaligned values
@@ -1507,8 +1538,9 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
 		gpios |= 3;
 
 		/* flip the buffer to output mode if required */
-		if (GPIO_SET_OUTPUT == function)
+		if (GPIO_SET_OUTPUT == function) {
 			stm32_gpiowrite(GPIO_GPIO_DIR, 1);
+		}
 	}
 
 #endif
@@ -1526,8 +1558,9 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
 				break;
 
 			case GPIO_SET_ALT_1:
-				if (_gpio_tab[i].alt != 0)
+				if (_gpio_tab[i].alt != 0) {
 					stm32_configgpio(_gpio_tab[i].alt);
+				}
 
 				break;
 			}
@@ -1537,8 +1570,9 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
 
 	/* flip buffer to input mode if required */
-	if ((GPIO_SET_INPUT == function) && (gpios & 3))
+	if ((GPIO_SET_INPUT == function) && (gpios & 3)) {
 		stm32_gpiowrite(GPIO_GPIO_DIR, 0);
+	}
 
 #endif
 }
@@ -1549,8 +1583,9 @@ PX4FMU::gpio_write(uint32_t gpios, int function)
 	int value = (function == GPIO_SET) ? 1 : 0;
 
 	for (unsigned i = 0; i < _ngpio; i++)
-		if (gpios & (1 << i))
+		if (gpios & (1 << i)) {
 			stm32_gpiowrite(_gpio_tab[i].output, value);
+		}
 }
 
 uint32_t
@@ -1559,8 +1594,9 @@ PX4FMU::gpio_read(void)
 	uint32_t bits = 0;
 
 	for (unsigned i = 0; i < _ngpio; i++)
-		if (stm32_gpioread(_gpio_tab[i].input))
+		if (stm32_gpioread(_gpio_tab[i].input)) {
 			bits |= (1 << i);
+		}
 
 	return bits;
 }
@@ -1664,6 +1700,7 @@ fmu_new_mode(PortMode new_mode)
 		break;
 
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+
 	case PORT_PWM4:
 		/* select 4-pin PWM mode */
 		servo_mode = PX4FMU::MODE_4PWM;
@@ -1701,8 +1738,9 @@ fmu_new_mode(PortMode new_mode)
 	}
 
 	/* adjust GPIO config for serial mode(s) */
-	if (gpio_bits != 0)
+	if (gpio_bits != 0) {
 		g_fmu->ioctl(0, GPIO_SET_ALT_1, gpio_bits);
+	}
 
 	/* (re)set the PWM output mode */
 	g_fmu->set_mode(servo_mode);
@@ -1801,10 +1839,11 @@ test(void)
 
 	fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
 
-	if (fd < 0)
+	if (fd < 0) {
 		errx(1, "open fail");
+	}
 
-	if (ioctl(fd, PWM_SERVO_ARM, 0) < 0)       err(1, "servo arm failed");
+	if (ioctl(fd, PWM_SERVO_ARM, 0) < 0) { err(1, "servo arm failed"); }
 
 	if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) {
 		err(1, "Unable to get servo count\n");
@@ -1822,8 +1861,9 @@ test(void)
 		/* sweep all servos between 1000..2000 */
 		servo_position_t servos[servo_count];
 
-		for (unsigned i = 0; i < servo_count; i++)
+		for (unsigned i = 0; i < servo_count; i++) {
 			servos[i] = pwm_value;
+		}
 
 		if (direction == 1) {
 			// use ioctl interface for one direction
@@ -1837,8 +1877,9 @@ test(void)
 			// and use write interface for the other direction
 			ret = write(fd, servos, sizeof(servos));
 
-			if (ret != (int)sizeof(servos))
+			if (ret != (int)sizeof(servos)) {
 				err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret);
+			}
 		}
 
 		if (direction > 0) {
@@ -1862,11 +1903,13 @@ test(void)
 		for (unsigned i = 0; i < servo_count; i++) {
 			servo_position_t value;
 
-			if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value))
+			if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value)) {
 				err(1, "error reading PWM servo %d", i);
+			}
 
-			if (value != servos[i])
+			if (value != servos[i]) {
 				errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]);
+			}
 		}
 
 		/* Check if user wants to quit */
@@ -1892,8 +1935,9 @@ test(void)
 void
 fake(int argc, char *argv[])
 {
-	if (argc < 5)
+	if (argc < 5) {
 		errx(1, "fmu fake <roll> <pitch> <yaw> <thrust> (values -100 .. 100)");
+	}
 
 	actuator_controls_s ac;
 
@@ -1907,8 +1951,9 @@ fake(int argc, char *argv[])
 
 	orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac);
 
-	if (handle == nullptr)
+	if (handle == nullptr) {
 		errx(1, "advertise failed");
+	}
 
 	actuator_armed_s aa;
 
@@ -1917,8 +1962,9 @@ fake(int argc, char *argv[])
 
 	handle = orb_advertise(ORB_ID(actuator_armed), &aa);
 
-	if (handle == nullptr)
+	if (handle == nullptr) {
 		errx(1, "advertise failed 2");
+	}
 
 	exit(0);
 }
@@ -1948,8 +1994,9 @@ fmu_main(int argc, char *argv[])
 	}
 
 
-	if (fmu_start() != OK)
+	if (fmu_start() != OK) {
 		errx(1, "failed to start the FMU driver");
+	}
 
 	/*
 	 * Mode switches.
@@ -1961,6 +2008,7 @@ fmu_main(int argc, char *argv[])
 		new_mode = PORT_FULL_PWM;
 
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+
 	} else if (!strcmp(verb, "mode_pwm4")) {
 		new_mode = PORT_PWM4;
 #endif
@@ -1984,19 +2032,22 @@ fmu_main(int argc, char *argv[])
 	if (new_mode != PORT_MODE_UNSET) {
 
 		/* yes but it's the same mode */
-		if (new_mode == g_port_mode)
+		if (new_mode == g_port_mode) {
 			return OK;
+		}
 
 		/* switch modes */
 		int ret = fmu_new_mode(new_mode);
 		exit(ret == OK ? 0 : 1);
 	}
 
-	if (!strcmp(verb, "test"))
+	if (!strcmp(verb, "test")) {
 		test();
+	}
 
-	if (!strcmp(verb, "fake"))
+	if (!strcmp(verb, "fake")) {
 		fake(argc - 1, argv + 1);
+	}
 
 	if (!strcmp(verb, "sensor_reset")) {
 		if (argc > 2) {
@@ -2035,6 +2086,7 @@ fmu_main(int argc, char *argv[])
 			}
 
 			exit(0);
+
 		} else {
 			warnx("i2c cmd args: <bus id> <clock Hz>");
 		}
@@ -2042,7 +2094,8 @@ fmu_main(int argc, char *argv[])
 
 	fprintf(stderr, "FMU: unrecognised command %s, try:\n", verb);
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
-	fprintf(stderr, "  mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test, fake, sensor_reset, id\n");
+	fprintf(stderr,
+		"  mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test, fake, sensor_reset, id\n");
 #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_AEROCORE)
 	fprintf(stderr, "  mode_gpio, mode_pwm, mode_pwm4, test, sensor_reset [milliseconds], i2c <bus> <hz>\n");
 #endif

src/drivers/px4io/px4io_i2c.cpp

@@ -31,11 +31,11 @@
  *
  ****************************************************************************/
 
- /**
-  * @file px4io_i2c.cpp
-  *
-  * I2C interface for PX4IO
-  */
+/**
+ * @file px4io_i2c.cpp
+ *
+ * I2C interface for PX4IO
+ */
 
 /* XXX trim includes */
 #include <px4_config.h>
@@ -94,8 +94,10 @@ PX4IO_I2C::init()
 	int ret;
 
 	ret = I2C::init();
-	if (ret != OK)
+
+	if (ret != OK) {
 		goto out;
+	}
 
 	/* XXX really should do something more here */
 
@@ -133,8 +135,11 @@ PX4IO_I2C::write(unsigned address, void *data, unsigned count)
 	msgv[1].length = 2 * count;
 
 	int ret = transfer(msgv, 2);
-	if (ret == OK)
+
+	if (ret == OK) {
 		ret = count;
+	}
+
 	return ret;
 }
 
@@ -161,8 +166,11 @@ PX4IO_I2C::read(unsigned address, void *data, unsigned count)
 	msgv[1].length = 2 * count;
 
 	int ret = transfer(msgv, 2);
-	if (ret == OK)
+
+	if (ret == OK) {
 		ret = count;
+	}
+
 	return ret;
 }
 

src/drivers/px4io/px4io_serial.cpp

@@ -31,11 +31,11 @@
  *
  ****************************************************************************/
 
- /**
-  * @file px4io_serial.cpp
-  *
-  * Serial interface for PX4IO
-  */
+/**
+ * @file px4io_serial.cpp
+ *
+ * Serial interface for PX4IO
+ */
 
 /* XXX trim includes */
 #include <px4_config.h>
@@ -159,7 +159,7 @@ private:
 
 	/* do not allow top copying this class */
 	PX4IO_serial(PX4IO_serial &);
-	PX4IO_serial& operator = (const PX4IO_serial &);
+	PX4IO_serial &operator = (const PX4IO_serial &);
 
 };
 
@@ -199,6 +199,7 @@ PX4IO_serial::~PX4IO_serial()
 		stm32_dmastop(_tx_dma);
 		stm32_dmafree(_tx_dma);
 	}
+
 	if (_rx_dma != nullptr) {
 		stm32_dmastop(_rx_dma);
 		stm32_dmafree(_rx_dma);
@@ -232,8 +233,9 @@ PX4IO_serial::~PX4IO_serial()
 	perf_free(_pc_idle);
 	perf_free(_pc_badidle);
 
-	if (g_interface == this)
+	if (g_interface == this) {
 		g_interface = nullptr;
+	}
 }
 
 int
@@ -243,6 +245,7 @@ PX4IO_serial::init()
 	/* allocate DMA */
 	_tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP);
 	_rx_dma = stm32_dmachannel(PX4IO_SERIAL_RX_DMAMAP);
+
 	if ((_tx_dma == nullptr) || (_rx_dma == nullptr)) {
 		return -1;
 	}
@@ -305,19 +308,22 @@ PX4IO_serial::ioctl(unsigned operation, unsigned &arg)
 			for (;;) {
 				while (!(rSR & USART_SR_TXE))
 					;
+
 				rDR = 0x55;
 			}
+
 			return 0;
 
-		case 1:
-			{
+		case 1: {
 				unsigned fails = 0;
+
 				for (unsigned count = 0;; count++) {
 					uint16_t value = count & 0xffff;
 
-					if (write((PX4IO_PAGE_TEST << 8) | PX4IO_P_TEST_LED, &value, 1) != 0)
+					if (write((PX4IO_PAGE_TEST << 8) | PX4IO_P_TEST_LED, &value, 1) != 0) {
 						fails++;
-						
+					}
+
 					if (count >= 5000) {
 						lowsyslog("==== test 1 : %u failures ====\n", fails);
 						perf_print_counter(_pc_txns);
@@ -333,12 +339,15 @@ PX4IO_serial::ioctl(unsigned operation, unsigned &arg)
 						count = 0;
 					}
 				}
+
 				return 0;
 			}
+
 		case 2:
 			lowsyslog("test 2\n");
 			return 0;
 		}
+
 	default:
 		break;
 	}
@@ -353,20 +362,24 @@ PX4IO_serial::write(unsigned address, void *data, unsigned count)
 	uint8_t offset = address & 0xff;
 	const uint16_t *values = reinterpret_cast<const uint16_t *>(data);
 
-	if (count > PKT_MAX_REGS)
+	if (count > PKT_MAX_REGS) {
 		return -EINVAL;
+	}
 
 	sem_wait(&_bus_semaphore);
 
 	int result;
+
 	for (unsigned retries = 0; retries < 3; retries++) {
 
 		_dma_buffer.count_code = count | PKT_CODE_WRITE;
 		_dma_buffer.page = page;
 		_dma_buffer.offset = offset;
 		memcpy((void *)&_dma_buffer.regs[0], (void *)values, (2 * count));
-		for (unsigned i = count; i < PKT_MAX_REGS; i++)
+
+		for (unsigned i = count; i < PKT_MAX_REGS; i++) {
 			_dma_buffer.regs[i] = 0x55aa;
+		}
 
 		/* XXX implement check byte */
 
@@ -386,13 +399,16 @@ PX4IO_serial::write(unsigned address, void *data, unsigned count)
 
 			break;
 		}
+
 		perf_count(_pc_retries);
 	}
 
 	sem_post(&_bus_semaphore);
 
-	if (result == OK)
+	if (result == OK) {
 		result = count;
+	}
+
 	return result;
 }
 
@@ -403,12 +419,14 @@ PX4IO_serial::read(unsigned address, void *data, unsigned count)
 	uint8_t offset = address & 0xff;
 	uint16_t *values = reinterpret_cast<uint16_t *>(data);
 
-	if (count > PKT_MAX_REGS)
+	if (count > PKT_MAX_REGS) {
 		return -EINVAL;
+	}
 
 	sem_wait(&_bus_semaphore);
 
 	int result;
+
 	for (unsigned retries = 0; retries < 3; retries++) {
 
 		_dma_buffer.count_code = count | PKT_CODE_READ;
@@ -428,14 +446,16 @@ PX4IO_serial::read(unsigned address, void *data, unsigned count)
 				result = -EINVAL;
 				perf_count(_pc_protoerrs);
 
-			/* compare the received count with the expected count */
+				/* compare the received count with the expected count */
+
 			} else if (PKT_COUNT(_dma_buffer) != count) {
 
 				/* IO returned the wrong number of registers - no point retrying */
 				result = -EIO;
 				perf_count(_pc_protoerrs);
 
-			/* successful read */				
+				/* successful read */
+
 			} else {
 
 				/* copy back the result */
@@ -444,13 +464,16 @@ PX4IO_serial::read(unsigned address, void *data, unsigned count)
 
 			break;
 		}
+
 		perf_count(_pc_retries);
 	}
 
 	sem_post(&_bus_semaphore);
 
-	if (result == OK)
+	if (result == OK) {
 		result = count;
+	}
+
 	return result;
 }
 
@@ -517,14 +540,16 @@ PX4IO_serial::_wait_complete()
 	/* compute the deadline for a 10ms timeout */
 	struct timespec abstime;
 	clock_gettime(CLOCK_REALTIME, &abstime);
-	abstime.tv_nsec += 10*1000*1000;
-	if (abstime.tv_nsec >= 1000*1000*1000) {
+	abstime.tv_nsec += 10 * 1000 * 1000;
+
+	if (abstime.tv_nsec >= 1000 * 1000 * 1000) {
 		abstime.tv_sec++;
-		abstime.tv_nsec -= 1000*1000*1000;
+		abstime.tv_nsec -= 1000 * 1000 * 1000;
 	}
 
 	/* wait for the transaction to complete - 64 bytes @ 1.5Mbps ~426µs */
 	int ret;
+
 	for (;;) {
 		ret = sem_timedwait(&_completion_semaphore, &abstime);
 
@@ -539,6 +564,7 @@ PX4IO_serial::_wait_complete()
 			/* check packet CRC - corrupt packet errors mean IO receive CRC error */
 			uint8_t crc = _dma_buffer.crc;
 			_dma_buffer.crc = 0;
+
 			if ((crc != crc_packet(&_dma_buffer)) | (PKT_CODE(_dma_buffer) == PKT_CODE_CORRUPT)) {
 				perf_count(_pc_crcerrs);
 				ret = -EIO;
@@ -588,6 +614,7 @@ PX4IO_serial::_do_rx_dma_callback(unsigned status)
 
 		/* check for packet overrun - this will occur after DMA completes */
 		uint32_t sr = rSR;
+
 		if (sr & (USART_SR_ORE | USART_SR_RXNE)) {
 			(void)rDR;
 			status = DMA_STATUS_TEIF;
@@ -607,8 +634,10 @@ PX4IO_serial::_do_rx_dma_callback(unsigned status)
 int
 PX4IO_serial::_interrupt(int irq, void *context)
 {
-	if (g_interface != nullptr)
+	if (g_interface != nullptr) {
 		g_interface->_do_interrupt();
+	}
+
 	return 0;
 }
 
@@ -619,10 +648,10 @@ PX4IO_serial::_do_interrupt()
 	(void)rDR;		/* read DR to clear status */
 
 	if (sr & (USART_SR_ORE |	/* overrun error - packet was too big for DMA or DMA was too slow */
-		USART_SR_NE |		/* noise error - we have lost a byte due to noise */
-		USART_SR_FE)) {		/* framing error - start/stop bit lost or line break */
-		
-		/* 
+		  USART_SR_NE |		/* noise error - we have lost a byte due to noise */
+		  USART_SR_FE)) {		/* framing error - start/stop bit lost or line break */
+
+		/*
 		 * If we are in the process of listening for something, these are all fatal;
 		 * abort the DMA with an error.
 		 */
@@ -649,6 +678,7 @@ PX4IO_serial::_do_interrupt()
 
 			/* verify that the received packet is complete */
 			size_t length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma);
+
 			if ((length < 1) || (length < PKT_SIZE(_dma_buffer))) {
 				perf_count(_pc_badidle);
 

src/drivers/px4io/px4io_uploader.cpp

@@ -126,8 +126,10 @@ PX4IO_Uploader::upload(const char *filenames[])
 	/* look for the bootloader for 150 ms */
 	for (int i = 0; i < 15; i++) {
 		ret = sync();
+
 		if (ret == OK) {
 			break;
+
 		} else {
 			usleep(10000);
 		}
@@ -143,6 +145,7 @@ PX4IO_Uploader::upload(const char *filenames[])
 	}
 
 	struct stat st;
+
 	if (stat(filename, &st) != 0) {
 		log("Failed to stat %s - %d\n", filename, (int)errno);
 		tcsetattr(_io_fd, TCSANOW, &t_original);
@@ -150,6 +153,7 @@ PX4IO_Uploader::upload(const char *filenames[])
 		_io_fd = -1;
 		return -errno;
 	}
+
 	fw_size = st.st_size;
 
 	if (_fw_fd == -1) {
@@ -180,6 +184,7 @@ PX4IO_Uploader::upload(const char *filenames[])
 		if (ret == OK) {
 			if (bl_rev <= BL_REV) {
 				log("found bootloader revision: %d", bl_rev);
+
 			} else {
 				log("found unsupported bootloader revision %d, exiting", bl_rev);
 				tcsetattr(_io_fd, TCSANOW, &t_original);
@@ -205,6 +210,7 @@ PX4IO_Uploader::upload(const char *filenames[])
 
 		if (bl_rev <= 2) {
 			ret = verify_rev2(fw_size);
+
 		} else {
 			/* verify rev 3 and higher. Every version *needs* to be verified. */
 			ret = verify_rev3(fw_size);
@@ -240,7 +246,7 @@ PX4IO_Uploader::upload(const char *filenames[])
 
 	// sleep for enough time for the IO chip to boot. This makes
 	// forceupdate more reliably startup IO again after update
-	up_udelay(100*1000);
+	up_udelay(100 * 1000);
 
 	return ret;
 }
@@ -274,12 +280,15 @@ int
 PX4IO_Uploader::recv_bytes(uint8_t *p, unsigned count)
 {
 	int ret = OK;
+
 	while (count--) {
 		ret = recv_byte_with_timeout(p++, 5000);
 
-		if (ret != OK)
+		if (ret != OK) {
 			break;
+		}
 	}
+
 	return ret;
 }
 
@@ -296,9 +305,11 @@ PX4IO_Uploader::drain()
 		ret = recv_byte_with_timeout(&c, 40);
 
 #ifdef UDEBUG
+
 		if (ret == OK) {
 			log("discard 0x%02x", c);
 		}
+
 #endif
 	} while (ret == OK);
 }
@@ -309,8 +320,11 @@ PX4IO_Uploader::send(uint8_t c)
 #ifdef UDEBUG
 	log("send 0x%02x", c);
 #endif
-	if (write(_io_fd, &c, 1) != 1)
+
+	if (write(_io_fd, &c, 1) != 1) {
 		return -errno;
+	}
+
 	return OK;
 }
 
@@ -318,11 +332,15 @@ int
 PX4IO_Uploader::send(uint8_t *p, unsigned count)
 {
 	int ret;
+
 	while (count--) {
 		ret = send(*p++);
-		if (ret != OK)
+
+		if (ret != OK) {
 			break;
+		}
 	}
+
 	return ret;
 }
 
@@ -334,13 +352,15 @@ PX4IO_Uploader::get_sync(unsigned timeout)
 
 	ret = recv_byte_with_timeout(c, timeout);
 
-	if (ret != OK)
+	if (ret != OK) {
 		return ret;
+	}
 
 	ret = recv_byte_with_timeout(c + 1, timeout);
 
-	if (ret != OK)
+	if (ret != OK) {
 		return ret;
+	}
 
 	if ((c[0] != PROTO_INSYNC) || (c[1] != PROTO_OK)) {
 		log("bad sync 0x%02x,0x%02x", c[0], c[1]);
@@ -356,8 +376,9 @@ PX4IO_Uploader::sync()
 	drain();
 
 	/* complete any pending program operation */
-	for (unsigned i = 0; i < (PROG_MULTI_MAX + 6); i++)
+	for (unsigned i = 0; i < (PROG_MULTI_MAX + 6); i++) {
 		send(0);
+	}
 
 	send(PROTO_GET_SYNC);
 	send(PROTO_EOC);
@@ -375,8 +396,9 @@ PX4IO_Uploader::get_info(int param, uint32_t &val)
 
 	ret = recv_bytes((uint8_t *)&val, sizeof(val));
 
-	if (ret != OK)
+	if (ret != OK) {
 		return ret;
+	}
 
 	return get_sync();
 }
@@ -395,14 +417,17 @@ static int read_with_retry(int fd, void *buf, size_t n)
 {
 	int ret;
 	uint8_t retries = 0;
+
 	do {
 		ret = read(fd, buf, n);
 	} while (ret == -1 && retries++ < 100);
+
 	if (retries != 0) {
 		printf("read of %u bytes needed %u retries\n",
 		       (unsigned)n,
 		       (unsigned)retries);
 	}
+
 	return ret;
 }
 
@@ -415,6 +440,7 @@ PX4IO_Uploader::program(size_t fw_size)
 	size_t sent = 0;
 
 	file_buf = new uint8_t[PROG_MULTI_MAX];
+
 	if (!file_buf) {
 		log("Can't allocate program buffer");
 		return -ENOMEM;
@@ -430,13 +456,15 @@ PX4IO_Uploader::program(size_t fw_size)
 	while (sent < fw_size) {
 		/* get more bytes to program */
 		size_t n = fw_size - sent;
+
 		if (n > PROG_MULTI_MAX) {
 			n = PROG_MULTI_MAX;
 		}
+
 		count = read_with_retry(_fw_fd, file_buf, n);
 
 		if (count != (ssize_t)n) {
-			log("firmware read of %u bytes at %u failed -> %d errno %d", 
+			log("firmware read of %u bytes at %u failed -> %d errno %d",
 			    (unsigned)n,
 			    (unsigned)sent,
 			    (int)count,
@@ -478,32 +506,37 @@ PX4IO_Uploader::verify_rev2(size_t fw_size)
 	send(PROTO_EOC);
 	ret = get_sync();
 
-	if (ret != OK)
+	if (ret != OK) {
 		return ret;
+	}
 
 	while (sent < fw_size) {
 		/* get more bytes to verify */
 		size_t n = fw_size - sent;
+
 		if (n > sizeof(file_buf)) {
 			n = sizeof(file_buf);
 		}
+
 		count = read_with_retry(_fw_fd, file_buf, n);
 
 		if (count != (ssize_t)n) {
-			log("firmware read of %u bytes at %u failed -> %d errno %d", 
+			log("firmware read of %u bytes at %u failed -> %d errno %d",
 			    (unsigned)n,
 			    (unsigned)sent,
 			    (int)count,
 			    (int)errno);
 		}
 
-		if (count == 0)
+		if (count == 0) {
 			break;
+		}
 
 		sent += count;
 
-		if (count < 0)
+		if (count < 0) {
 			return -errno;
+		}
 
 		ASSERT((count % 4) == 0);
 
@@ -564,13 +597,15 @@ PX4IO_Uploader::verify_rev3(size_t fw_size_local)
 	/* read through the firmware file again and calculate the checksum*/
 	while (bytes_read < fw_size_local) {
 		size_t n = fw_size_local - bytes_read;
+
 		if (n > sizeof(file_buf)) {
 			n = sizeof(file_buf);
 		}
+
 		count = read_with_retry(_fw_fd, file_buf, n);
 
 		if (count != (ssize_t)n) {
-			log("firmware read of %u bytes at %u failed -> %d errno %d", 
+			log("firmware read of %u bytes at %u failed -> %d errno %d",
 			    (unsigned)n,
 			    (unsigned)bytes_read,
 			    (int)count,
@@ -581,9 +616,11 @@ PX4IO_Uploader::verify_rev3(size_t fw_size_local)
 		if (count == 0) {
 			break;
 		}
+
 		/* stop if the file cannot be read */
-		if (count < 0)
+		if (count < 0) {
 			return -errno;
+		}
 
 		/* calculate crc32 sum */
 		sum = crc32part((uint8_t *)&file_buf, sizeof(file_buf), sum);
@@ -601,7 +638,7 @@ PX4IO_Uploader::verify_rev3(size_t fw_size_local)
 	send(PROTO_GET_CRC);
 	send(PROTO_EOC);
 
-	ret = recv_bytes((uint8_t*)(&crc), sizeof(crc));
+	ret = recv_bytes((uint8_t *)(&crc), sizeof(crc));
 
 	if (ret != OK) {
 		log("did not receive CRC checksum");
@@ -621,7 +658,7 @@ int
 PX4IO_Uploader::reboot()
 {
 	send(PROTO_REBOOT);
-	up_udelay(100*1000); // Ensure the farend is in wait for char.
+	up_udelay(100 * 1000); // Ensure the farend is in wait for char.
 	send(PROTO_EOC);
 
 	return OK;

src/drivers/rgbled/module.mk

@@ -4,6 +4,7 @@
 
 MODULE_COMMAND	 = rgbled
 
-SRCS		 = rgbled.cpp
+SRCS		 = rgbled.cpp \
+			   rgbled_params.c
 
 MAXOPTIMIZATION	 = -Os

src/drivers/rgbled/rgbled.cpp

@@ -101,11 +101,13 @@ private:
 	uint8_t			_g;
 	uint8_t			_b;
 	float			_brightness;
+	float			_max_brightness;
 
 	bool			_running;
 	int			_led_interval;
 	bool			_should_run;
 	int			_counter;
+	int			_param_sub;
 
 	void 			set_color(rgbled_color_t ledcolor);
 	void			set_mode(rgbled_mode_t mode);
@@ -117,6 +119,7 @@ private:
 	int			send_led_enable(bool enable);
 	int			send_led_rgb();
 	int			get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b);
+	void		update_params();
 };
 
 /* for now, we only support one RGBLED */
@@ -140,10 +143,12 @@ RGBLED::RGBLED(int bus, int rgbled) :
 	_g(0),
 	_b(0),
 	_brightness(1.0f),
+	_max_brightness(1.0f),
 	_running(false),
 	_led_interval(0),
 	_should_run(false),
-	_counter(0)
+	_counter(0),
+	_param_sub(-1)
 {
 	memset(&_work, 0, sizeof(_work));
 	memset(&_pattern, 0, sizeof(_pattern));
@@ -285,6 +290,22 @@ RGBLED::led()
 		return;
 	}
 
+	if (_param_sub < 0) {
+		_param_sub = orb_subscribe(ORB_ID(parameter_update));
+	}
+
+	if (_param_sub >= 0) {
+		bool updated = false;
+		orb_check(_param_sub, &updated);
+		if (updated) {
+			parameter_update_s pupdate;
+			orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate);
+			update_params();
+			// Immediately update to change brightness
+			send_led_rgb();
+		}
+	}
+
 	switch (_mode) {
 	case RGBLED_MODE_BLINK_SLOW:
 	case RGBLED_MODE_BLINK_NORMAL:
@@ -546,9 +567,9 @@ RGBLED::send_led_rgb()
 {
 	/* To scale from 0..255 -> 0..15 shift right by 4 bits */
 	const uint8_t msg[6] = {
-		SUB_ADDR_PWM0, (uint8_t)((int)(_b * _brightness) >> 4),
-		SUB_ADDR_PWM1, (uint8_t)((int)(_g * _brightness) >> 4),
-		SUB_ADDR_PWM2, (uint8_t)((int)(_r * _brightness) >> 4)
+		SUB_ADDR_PWM0, static_cast<uint8_t>((_b >> 4) * _brightness * _max_brightness + 0.5f),
+		SUB_ADDR_PWM1, static_cast<uint8_t>((_g >> 4) * _brightness * _max_brightness + 0.5f),
+		SUB_ADDR_PWM2, static_cast<uint8_t>((_r >> 4) * _brightness * _max_brightness + 0.5f)
 	};
 	return transfer(msg, sizeof(msg), nullptr, 0);
 }
@@ -573,6 +594,21 @@ RGBLED::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b)
 	return ret;
 }
 
+void
+RGBLED::update_params()
+{
+	int32_t maxbrt = 15;
+	param_get(param_find("LED_RGB_MAXBRT"), &maxbrt);
+	maxbrt = maxbrt > 15 ? 15 : maxbrt;
+	maxbrt = maxbrt <  0 ?  0 : maxbrt;
+
+	// A minimum of 2 "on" steps is required for breathe effect
+	if (maxbrt == 1) {
+		maxbrt = 2;
+	}
+	_max_brightness = maxbrt / 15.0f;
+}
+
 void
 rgbled_usage()
 {

src/drivers/rgbled/rgbled_params.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015 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
@@ -31,51 +31,24 @@
  *
  ****************************************************************************/
 
-#include <uavcan_stm32/uavcan_stm32.hpp>
-#include <drivers/drv_hrt.h>
-
-/**
- * @file uavcan_clock.cpp
+/*
+ * @file rgbled_params.c
  *
- * Implements a clock for the CAN node.
+ * Parameters defined by the RBG led driver
  *
- * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ * @author Nate Weibley <nate.weibley@prioria.com>
  */
 
-namespace uavcan_stm32
-{
-namespace clock
-{
 
-uavcan::MonotonicTime getMonotonic()
-{
-	return uavcan::MonotonicTime::fromUSec(hrt_absolute_time());
-}
-
-uavcan::UtcTime getUtc()
-{
-	return uavcan::UtcTime();
-}
-
-void adjustUtc(uavcan::UtcDuration adjustment)
-{
-	(void)adjustment;
-}
-
-uavcan::uint64_t getUtcUSecFromCanInterrupt();
-
-uavcan::uint64_t getUtcUSecFromCanInterrupt()
-{
-	return 0;
-}
-
-} // namespace clock
-
-SystemClock &SystemClock::instance()
-{
-	static SystemClock inst;
-	return inst;
-}
-
-}
+#include <px4_config.h>
+#include <systemlib/param/param.h>
 
+/**
+ * RGB Led brightness limit
+ *
+ * Set to 0 to disable, 1 for minimum brightness up to 15 (max)
+ *
+ * @min 0
+ * @max 15
+ */
+PARAM_DEFINE_INT32(LED_RGB_MAXBRT, 15);

src/drivers/stm32/adc/adc.cpp

@@ -115,7 +115,7 @@ protected:
 
 private:
 	static const hrt_abstime _tickrate = 10000;	/**< 100Hz base rate */
-	
+
 	hrt_call		_call;
 	perf_counter_t		_sample_perf;
 
@@ -161,11 +161,13 @@ ADC::ADC(uint32_t channels) :
 			_channel_count++;
 		}
 	}
+
 	_samples = new adc_msg_s[_channel_count];
 
 	/* prefill the channel numbers in the sample array */
 	if (_samples != nullptr) {
 		unsigned index = 0;
+
 		for (unsigned i = 0; i < 32; i++) {
 			if (channels & (1 << i)) {
 				_samples[index].am_channel = i;
@@ -178,8 +180,9 @@ ADC::ADC(uint32_t channels) :
 
 ADC::~ADC()
 {
-	if (_samples != nullptr)
+	if (_samples != nullptr) {
 		delete _samples;
+	}
 }
 
 int
@@ -189,8 +192,11 @@ ADC::init()
 #ifdef ADC_CR2_CAL
 	rCR2 |= ADC_CR2_CAL;
 	usleep(100);
-	if (rCR2 & ADC_CR2_CAL)
+
+	if (rCR2 & ADC_CR2_CAL) {
 		return -1;
+	}
+
 #endif
 
 	/* arbitrarily configure all channels for 55 cycle sample time */
@@ -201,7 +207,7 @@ ADC::init()
 	rCR1 = 0;
 
 	/* enable the temperature sensor / Vrefint channel if supported*/
-	rCR2 = 
+	rCR2 =
 #ifdef ADC_CR2_TSVREFE
 		/* enable the temperature sensor in CR2 */
 		ADC_CR2_TSVREFE |
@@ -216,7 +222,7 @@ ADC::init()
 	/* configure for a single-channel sequence */
 	rSQR1 = 0;
 	rSQR2 = 0;
-	rSQR3 = 0;	/* will be updated with the channel each tick */ 
+	rSQR3 = 0;	/* will be updated with the channel each tick */
 
 	/* power-cycle the ADC and turn it on */
 	rCR2 &= ~ADC_CR2_ADON;
@@ -229,6 +235,7 @@ ADC::init()
 	/* kick off a sample and wait for it to complete */
 	hrt_abstime now = hrt_absolute_time();
 	rCR2 |= ADC_CR2_SWSTART;
+
 	while (!(rSR & ADC_SR_EOC)) {
 
 		/* don't wait for more than 500us, since that means something broke - should reset here if we see this */
@@ -256,8 +263,9 @@ ADC::read(file *filp, char *buffer, size_t len)
 {
 	const size_t maxsize = sizeof(adc_msg_s) * _channel_count;
 
-	if (len > maxsize)
+	if (len > maxsize) {
 		len = maxsize;
+	}
 
 	/* block interrupts while copying samples to avoid racing with an update */
 	irqstate_t flags = irqsave();
@@ -296,8 +304,10 @@ void
 ADC::_tick()
 {
 	/* scan the channel set and sample each */
-	for (unsigned i = 0; i < _channel_count; i++)
+	for (unsigned i = 0; i < _channel_count; i++) {
 		_samples[i].am_data = _sample(_samples[i].am_channel);
+	}
+
 	update_system_power();
 }
 
@@ -309,6 +319,7 @@ ADC::update_system_power(void)
 	system_power.timestamp = hrt_absolute_time();
 
 	system_power.voltage5V_v = 0;
+
 	for (unsigned i = 0; i < _channel_count; i++) {
 		if (_samples[i].am_channel == 4) {
 			// it is 2:1 scaled
@@ -331,9 +342,11 @@ ADC::update_system_power(void)
 	/* lazily publish */
 	if (_to_system_power != nullptr) {
 		orb_publish(ORB_ID(system_power), _to_system_power, &system_power);
+
 	} else {
 		_to_system_power = orb_advertise(ORB_ID(system_power), &system_power);
 	}
+
 #endif // CONFIG_ARCH_BOARD_PX4FMU_V2
 }
 
@@ -343,8 +356,9 @@ ADC::_sample(unsigned channel)
 	perf_begin(_sample_perf);
 
 	/* clear any previous EOC */
-	if (rSR & ADC_SR_EOC)
+	if (rSR & ADC_SR_EOC) {
 		rSR &= ~ADC_SR_EOC;
+	}
 
 	/* run a single conversion right now - should take about 60 cycles (a few microseconds) max */
 	rSQR3 = channel;
@@ -352,6 +366,7 @@ ADC::_sample(unsigned channel)
 
 	/* wait for the conversion to complete */
 	hrt_abstime now = hrt_absolute_time();
+
 	while (!(rSR & ADC_SR_EOC)) {
 
 		/* don't wait for more than 50us, since that means something broke - should reset here if we see this */
@@ -382,20 +397,23 @@ test(void)
 {
 
 	int fd = open(ADC0_DEVICE_PATH, O_RDONLY);
-	if (fd < 0)
+
+	if (fd < 0) {
 		err(1, "can't open ADC device");
+	}
 
 	for (unsigned i = 0; i < 50; i++) {
 		adc_msg_s data[12];
 		ssize_t count = read(fd, data, sizeof(data));
 
-		if (count < 0)
+		if (count < 0) {
 			errx(1, "read error");
+		}
 
 		unsigned channels = count / sizeof(data[0]);
 
 		for (unsigned j = 0; j < channels; j++) {
-			printf ("%d: %u  ", data[j].am_channel, data[j].am_data);
+			printf("%d: %u  ", data[j].am_channel, data[j].am_data);
 		}
 
 		printf("\n");
@@ -410,11 +428,12 @@ int
 adc_main(int argc, char *argv[])
 {
 	if (g_adc == nullptr) {
-        /* XXX this hardcodes the default channel set for the board in board_config.h - should be configurable */
-        g_adc = new ADC(ADC_CHANNELS);
+		/* XXX this hardcodes the default channel set for the board in board_config.h - should be configurable */
+		g_adc = new ADC(ADC_CHANNELS);
 
-		if (g_adc == nullptr)
+		if (g_adc == nullptr) {
 			errx(1, "couldn't allocate the ADC driver");
+		}
 
 		if (g_adc->init() != OK) {
 			delete g_adc;
@@ -423,8 +442,9 @@ adc_main(int argc, char *argv[])
 	}
 
 	if (argc > 1) {
-		if (!strcmp(argv[1], "test"))
+		if (!strcmp(argv[1], "test")) {
 			test();
+		}
 	}
 
 	exit(0);

src/drivers/stm32/drv_hrt.c

@@ -455,8 +455,9 @@ hrt_ppm_decode(uint32_t status)
 	unsigned i;
 
 	/* if we missed an edge, we have to give up */
-	if (status & SR_OVF_PPM)
+	if (status & SR_OVF_PPM) {
 		goto error;
+	}
 
 	/* how long since the last edge? - this handles counter wrapping implicitely. */
 	width = count - ppm.last_edge;
@@ -464,8 +465,10 @@ hrt_ppm_decode(uint32_t status)
 #if PPM_DEBUG
 	ppm_edge_history[ppm_edge_next++] = width;
 
-	if (ppm_edge_next >= 32)
+	if (ppm_edge_next >= 32) {
 		ppm_edge_next = 0;
+	}
+
 #endif
 
 	/*
@@ -501,8 +504,9 @@ hrt_ppm_decode(uint32_t status)
 		} 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++)
+				for (i = 0; i < ppm.next_channel; i++) {
 					ppm_buffer[i] = ppm_temp_buffer[i];
+				}
 
 				ppm_last_valid_decode = hrt_absolute_time();
 
@@ -527,8 +531,9 @@ hrt_ppm_decode(uint32_t status)
 	case ARM:
 
 		/* we expect a pulse giving us the first mark */
-		if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH)
-			goto error;		/* pulse was too short or too long */
+		if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH) {
+			goto error;        /* pulse was too short or too long */
+		}
 
 		/* record the mark timing, expect an inactive edge */
 		ppm.last_mark = ppm.last_edge;
@@ -542,8 +547,9 @@ hrt_ppm_decode(uint32_t status)
 	case INACTIVE:
 
 		/* we expect a short pulse */
-		if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH)
-			goto error;		/* pulse was too short or too long */
+		if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH) {
+			goto error;        /* pulse was too short or too long */
+		}
 
 		/* this edge is not interesting, but now we are ready for the next mark */
 		ppm.phase = ACTIVE;
@@ -557,17 +563,21 @@ hrt_ppm_decode(uint32_t status)
 #if PPM_DEBUG
 		ppm_pulse_history[ppm_pulse_next++] = interval;
 
-		if (ppm_pulse_next >= 32)
+		if (ppm_pulse_next >= 32) {
 			ppm_pulse_next = 0;
+		}
+
 #endif
 
 		/* if the mark-mark timing is out of bounds, abandon the frame */
-		if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE))
+		if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE)) {
 			goto error;
+		}
 
 		/* if we have room to store the value, do so */
-		if (ppm.next_channel < PPM_MAX_CHANNELS)
+		if (ppm.next_channel < PPM_MAX_CHANNELS) {
 			ppm_temp_buffer[ppm.next_channel++] = interval;
+		}
 
 		ppm.phase = INACTIVE;
 		break;
@@ -668,8 +678,9 @@ hrt_absolute_time(void)
 	 * This simple test is sufficient due to the guarantee that
 	 * we are always called at least once per counter period.
 	 */
-	if (count < last_count)
+	if (count < last_count) {
 		base_time += HRT_COUNTER_PERIOD;
+	}
 
 	/* save the count for next time */
 	last_count = count;
@@ -800,8 +811,9 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
 	   queue for the uninitialised entry->link but we don't do
 	   anything actually unsafe.
 	*/
-	if (entry->deadline != 0)
+	if (entry->deadline != 0) {
 		sq_rem(&entry->link, &callout_queue);
+	}
 
 	entry->deadline = deadline;
 	entry->period = interval;
@@ -883,11 +895,13 @@ hrt_call_invoke(void)
 
 		call = (struct hrt_call *)sq_peek(&callout_queue);
 
-		if (call == NULL)
+		if (call == NULL) {
 			break;
+		}
 
-		if (call->deadline > now)
+		if (call->deadline > now) {
 			break;
+		}
 
 		sq_rem(&call->link, &callout_queue);
 		//lldbg("call pop\n");

src/drivers/stm32/drv_pwm_servo.c

@@ -174,19 +174,22 @@ pwm_channel_init(unsigned channel)
 int
 up_pwm_servo_set(unsigned channel, servo_position_t value)
 {
-	if (channel >= PWM_SERVO_MAX_CHANNELS)
+	if (channel >= PWM_SERVO_MAX_CHANNELS) {
 		return -1;
+	}
 
 	unsigned timer = pwm_channels[channel].timer_index;
 
 	/* test timer for validity */
 	if ((pwm_timers[timer].base == 0) ||
-	    (pwm_channels[channel].gpio == 0))
+	    (pwm_channels[channel].gpio == 0)) {
 		return -1;
+	}
 
 	/* configure the channel */
-	if (value > 0)
+	if (value > 0) {
 		value--;
+	}
 
 	switch (pwm_channels[channel].timer_channel) {
 	case 1:
@@ -215,16 +218,18 @@ up_pwm_servo_set(unsigned channel, servo_position_t value)
 servo_position_t
 up_pwm_servo_get(unsigned channel)
 {
-	if (channel >= PWM_SERVO_MAX_CHANNELS)
+	if (channel >= PWM_SERVO_MAX_CHANNELS) {
 		return 0;
+	}
 
 	unsigned timer = pwm_channels[channel].timer_index;
 	servo_position_t value = 0;
 
 	/* test timer for validity */
 	if ((pwm_timers[timer].base == 0) ||
-	    (pwm_channels[channel].timer_channel == 0))
+	    (pwm_channels[channel].timer_channel == 0)) {
 		return 0;
+	}
 
 	/* configure the channel */
 	switch (pwm_channels[channel].timer_channel) {
@@ -253,15 +258,17 @@ up_pwm_servo_init(uint32_t channel_mask)
 {
 	/* do basic timer initialisation first */
 	for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) {
-		if (pwm_timers[i].base != 0)
+		if (pwm_timers[i].base != 0) {
 			pwm_timer_init(i);
+		}
 	}
 
 	/* now init channels */
 	for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) {
 		/* don't do init for disabled channels; this leaves the pin configs alone */
-		if (((1 << i) & channel_mask) && (pwm_channels[i].timer_channel != 0))
+		if (((1 << i) & channel_mask) && (pwm_channels[i].timer_channel != 0)) {
 			pwm_channel_init(i);
+		}
 	}
 
 	return OK;
@@ -278,13 +285,17 @@ int
 up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
 {
 	/* limit update rate to 1..10000Hz; somewhat arbitrary but safe */
-	if (rate < 1)
-		return -ERANGE;
-	if (rate > 10000)
+	if (rate < 1) {
 		return -ERANGE;
+	}
 
-	if ((group >= PWM_SERVO_MAX_TIMERS) || (pwm_timers[group].base == 0))
+	if (rate > 10000) {
+		return -ERANGE;
+	}
+
+	if ((group >= PWM_SERVO_MAX_TIMERS) || (pwm_timers[group].base == 0)) {
 		return ERROR;
+	}
 
 	pwm_timer_set_rate(group, rate);
 
@@ -294,8 +305,9 @@ up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
 int
 up_pwm_servo_set_rate(unsigned rate)
 {
-	for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++)
+	for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) {
 		up_pwm_servo_set_rate_group_update(i, rate);
+	}
 
 	return 0;
 }
@@ -306,9 +318,11 @@ up_pwm_servo_get_rate_group(unsigned group)
 	unsigned channels = 0;
 
 	for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) {
-		if ((pwm_channels[i].gpio != 0) && (pwm_channels[i].timer_index == group))
+		if ((pwm_channels[i].gpio != 0) && (pwm_channels[i].timer_index == group)) {
 			channels |= (1 << i);
+		}
 	}
+
 	return channels;
 }
 

src/drivers/stm32/tone_alarm/tone_alarm.cpp

@@ -47,16 +47,16 @@
  * From Wikibooks:
  *
  * PLAY "[string expression]"
- * 
+ *
  * Used to play notes and a score ... The tones are indicated by letters A through G.
- * Accidentals are indicated with a "+" or "#" (for sharp) or "-" (for flat) 
+ * Accidentals are indicated with a "+" or "#" (for sharp) or "-" (for flat)
  * immediately after the note letter. See this example:
- * 
+ *
  *   PLAY "C C# C C#"
  *
  * Whitespaces are ignored inside the string expression. There are also codes that
- * set the duration, octave and tempo. They are all case-insensitive. PLAY executes 
- * the commands or notes the order in which they appear in the string. Any indicators 
+ * set the duration, octave and tempo. They are all case-insensitive. PLAY executes
+ * the commands or notes the order in which they appear in the string. Any indicators
  * that change the properties are effective for the notes following that indicator.
  *
  * Ln     Sets the duration (length) of the notes. The variable n does not indicate an actual duration
@@ -66,15 +66,15 @@
  *        The shorthand notation of length is also provided for a note. For example, "L4 CDE L8 FG L4 AB"
  *        can be shortened to "L4 CDE F8G8 AB". F and G play as eighth notes while others play as quarter notes.
  * On     Sets the current octave. Valid values for n are 0 through 6. An octave begins with C and ends with B.
- *        Remember that C- is equivalent to B. 
+ *        Remember that C- is equivalent to B.
  * < >    Changes the current octave respectively down or up one level.
  * Nn     Plays a specified note in the seven-octave range. Valid values are from 0 to 84. (0 is a pause.)
  *        Cannot use with sharp and flat. Cannot use with the shorthand notation neither.
  * MN     Stand for Music Normal. Note duration is 7/8ths of the length indicated by Ln. It is the default mode.
  * ML     Stand for Music Legato. Note duration is full length of that indicated by Ln.
  * MS     Stand for Music Staccato. Note duration is 3/4ths of the length indicated by Ln.
- * Pn     Causes a silence (pause) for the length of note indicated (same as Ln). 
- * Tn     Sets the number of "L4"s per minute (tempo). Valid values are from 32 to 255. The default value is T120. 
+ * Pn     Causes a silence (pause) for the length of note indicated (same as Ln).
+ * Tn     Sets the number of "L4"s per minute (tempo). Valid values are from 32 to 255. The default value is T120.
  * .      When placed after a note, it causes the duration of the note to be 3/2 of the set duration.
  *        This is how to get "dotted" notes. "L4 C#." would play C sharp as a dotted quarter note.
  *        It can be used for a pause as well.
@@ -117,8 +117,23 @@
 
 #include <systemlib/err.h>
 
+/* Check that tone alarm and HRT timers are different */
+#if defined(TONE_ALARM_TIMER)  && defined(HRT_TIMER)
+# if TONE_ALARM_TIMER == HRT_TIMER
+#   error TONE_ALARM_TIMER and HRT_TIMER must use different timers.
+# endif
+#endif
+
 /* Tone alarm configuration */
-#if   TONE_ALARM_TIMER == 2
+#if   TONE_ALARM_TIMER == 1
+# define TONE_ALARM_BASE              STM32_TIM1_BASE
+# define TONE_ALARM_CLOCK             STM32_APB2_TIM1_CLKIN
+# define TONE_ALARM_CLOCK_POWER_REG   STM32_RCC_APB2ENR
+# define TONE_ALARM_CLOCK_ENABLE      RCC_APB2ENR_TIM1EN
+# ifdef CONFIG_STM32_TIM1
+#  error Must not set CONFIG_STM32_TIM1 when TONE_ALARM_TIMER is 1
+# endif
+#elif TONE_ALARM_TIMER == 2
 # define TONE_ALARM_BASE		STM32_TIM2_BASE
 # define TONE_ALARM_CLOCK		STM32_APB1_TIM2_CLKIN
 # define TONE_ALARM_CLOCK_POWER_REG   STM32_RCC_APB1ENR
@@ -150,6 +165,14 @@
 # ifdef CONFIG_STM32_TIM5
 #  error Must not set CONFIG_STM32_TIM5 when TONE_ALARM_TIMER is 5
 # endif
+#elif TONE_ALARM_TIMER == 8
+# define TONE_ALARM_BASE		STM32_TIM8_BASE
+# define TONE_ALARM_CLOCK		STM32_APB2_TIM8_CLKIN
+# define TONE_ALARM_CLOCK_POWER_REG   STM32_RCC_APB2ENR
+# define TONE_ALARM_CLOCK_ENABLE	RCC_APB2ENR_TIM8EN
+# ifdef CONFIG_STM32_TIM8
+#  error Must not set CONFIG_STM32_TIM8 when TONE_ALARM_TIMER is 8
+# endif
 #elif TONE_ALARM_TIMER == 9
 # define TONE_ALARM_BASE		STM32_TIM9_BASE
 # define TONE_ALARM_CLOCK		STM32_APB2_TIM9_CLKIN
@@ -175,7 +198,7 @@
 #  error Must not set CONFIG_STM32_TIM11 when TONE_ALARM_TIMER is 11
 # endif
 #else
-# error Must set TONE_ALARM_TIMER to a generic timer in order to use this driver.
+# error Must set TONE_ALARM_TIMER to one of the timers between 1 and 11 (inclusive) to use this driver.
 #endif
 
 #if TONE_ALARM_CHANNEL == 1
@@ -208,24 +231,47 @@
  */
 #define REG(_reg)	(*(volatile uint32_t *)(TONE_ALARM_BASE + _reg))
 
-#define rCR1     	REG(STM32_GTIM_CR1_OFFSET)
-#define rCR2     	REG(STM32_GTIM_CR2_OFFSET)
-#define rSMCR    	REG(STM32_GTIM_SMCR_OFFSET)
-#define rDIER    	REG(STM32_GTIM_DIER_OFFSET)
-#define rSR      	REG(STM32_GTIM_SR_OFFSET)
-#define rEGR     	REG(STM32_GTIM_EGR_OFFSET)
-#define rCCMR1   	REG(STM32_GTIM_CCMR1_OFFSET)
-#define rCCMR2   	REG(STM32_GTIM_CCMR2_OFFSET)
-#define rCCER    	REG(STM32_GTIM_CCER_OFFSET)
-#define rCNT     	REG(STM32_GTIM_CNT_OFFSET)
-#define rPSC     	REG(STM32_GTIM_PSC_OFFSET)
-#define rARR     	REG(STM32_GTIM_ARR_OFFSET)
-#define rCCR1    	REG(STM32_GTIM_CCR1_OFFSET)
-#define rCCR2    	REG(STM32_GTIM_CCR2_OFFSET)
-#define rCCR3    	REG(STM32_GTIM_CCR3_OFFSET)
-#define rCCR4    	REG(STM32_GTIM_CCR4_OFFSET)
-#define rDCR     	REG(STM32_GTIM_DCR_OFFSET)
-#define rDMAR    	REG(STM32_GTIM_DMAR_OFFSET)
+#if TONE_ALARM_TIMER == 1 || TONE_ALARM_TIMER == 8 // Note: If using TIM1 or TIM8, then you are using the ADVANCED timers and NOT the GENERAL TIMERS, therefore different registers
+# define rCR1         REG(STM32_ATIM_CR1_OFFSET)
+# define rCR2         REG(STM32_ATIM_CR2_OFFSET)
+# define rSMCR        REG(STM32_ATIM_SMCR_OFFSET)
+# define rDIER        REG(STM32_ATIM_DIER_OFFSET)
+# define rSR          REG(STM32_ATIM_SR_OFFSET)
+# define rEGR         REG(STM32_ATIM_EGR_OFFSET)
+# define rCCMR1       REG(STM32_ATIM_CCMR1_OFFSET)
+# define rCCMR2       REG(STM32_ATIM_CCMR2_OFFSET)
+# define rCCER        REG(STM32_ATIM_CCER_OFFSET)
+# define rCNT         REG(STM32_ATIM_CNT_OFFSET)
+# define rPSC         REG(STM32_ATIM_PSC_OFFSET)
+# define rARR         REG(STM32_ATIM_ARR_OFFSET)
+# define rRCR         REG(STM32_ATIM_RCR_OFFSET)
+# define rCCR1        REG(STM32_ATIM_CCR1_OFFSET)
+# define rCCR2        REG(STM32_ATIM_CCR2_OFFSET)
+# define rCCR3        REG(STM32_ATIM_CCR3_OFFSET)
+# define rCCR4        REG(STM32_ATIM_CCR4_OFFSET)
+# define rBDTR        REG(STM32_ATIM_BDTR_OFFSET)
+# define rDCR         REG(STM32_ATIM_DCR_OFFSET)
+# define rDMAR        REG(STM32_ATIM_DMAR_OFFSET)
+#else
+# define rCR1     	REG(STM32_GTIM_CR1_OFFSET)
+# define rCR2     	REG(STM32_GTIM_CR2_OFFSET)
+# define rSMCR    	REG(STM32_GTIM_SMCR_OFFSET)
+# define rDIER    	REG(STM32_GTIM_DIER_OFFSET)
+# define rSR      	REG(STM32_GTIM_SR_OFFSET)
+# define rEGR     	REG(STM32_GTIM_EGR_OFFSET)
+# define rCCMR1   	REG(STM32_GTIM_CCMR1_OFFSET)
+# define rCCMR2   	REG(STM32_GTIM_CCMR2_OFFSET)
+# define rCCER    	REG(STM32_GTIM_CCER_OFFSET)
+# define rCNT     	REG(STM32_GTIM_CNT_OFFSET)
+# define rPSC     	REG(STM32_GTIM_PSC_OFFSET)
+# define rARR     	REG(STM32_GTIM_ARR_OFFSET)
+# define rCCR1    	REG(STM32_GTIM_CCR1_OFFSET)
+# define rCCR2    	REG(STM32_GTIM_CCR2_OFFSET)
+# define rCCR3    	REG(STM32_GTIM_CCR3_OFFSET)
+# define rCCR4    	REG(STM32_GTIM_CCR4_OFFSET)
+# define rDCR     	REG(STM32_GTIM_DCR_OFFSET)
+# define rDMAR    	REG(STM32_GTIM_DMAR_OFFSET)
+#endif
 
 class ToneAlarm : public device::CDev
 {
@@ -237,14 +283,15 @@ public:
 
 	virtual int		ioctl(file *filp, int cmd, unsigned long arg);
 	virtual ssize_t		write(file *filp, const char *buffer, size_t len);
-	inline const char	*name(int tune) {
+	inline const char	*name(int tune)
+	{
 		return _tune_names[tune];
 	}
 
 private:
 	static const unsigned	_tune_max = 1024 * 8; // be reasonable about user tunes
-	const char		* _default_tunes[TONE_NUMBER_OF_TUNES];
-	const char		* _tune_names[TONE_NUMBER_OF_TUNES];
+	const char		 *_default_tunes[TONE_NUMBER_OF_TUNES];
+	const char		 *_tune_names[TONE_NUMBER_OF_TUNES];
 	static const uint8_t	_note_tab[];
 
 	unsigned		_default_tune_number; // number of currently playing default tune (0 for none)
@@ -268,8 +315,8 @@ private:
 	//
 	unsigned		note_to_divisor(unsigned note);
 
-	// Calculate the duration in microseconds of play and silence for a 
-	// note given the current tempo, length and mode and the number of 
+	// Calculate the duration in microseconds of play and silence for a
+	// note given the current tempo, length and mode and the number of
 	// dots following in the play string.
 	//
 	unsigned		note_duration(unsigned &silence, unsigned note_length, unsigned dots);
@@ -376,14 +423,15 @@ ToneAlarm::init()
 
 	ret = CDev::init();
 
-	if (ret != OK)
+	if (ret != OK) {
 		return ret;
+	}
 
 	/* configure the GPIO to the idle state */
 	stm32_configgpio(GPIO_TONE_ALARM_IDLE);
 
 	/* clock/power on our timer */
-    modifyreg32(TONE_ALARM_CLOCK_POWER_REG, 0, TONE_ALARM_CLOCK_ENABLE);
+	modifyreg32(TONE_ALARM_CLOCK_POWER_REG, 0, TONE_ALARM_CLOCK_ENABLE);
 
 	/* initialise the timer */
 	rCR1 = 0;
@@ -396,6 +444,10 @@ ToneAlarm::init()
 	rCCER = TONE_CCER;
 	rDCR = 0;
 
+#ifdef rBDTR // If using an advanced timer, you need to activate the output
+	rBDTR = ATIM_BDTR_MOE; // enable the main output of the advanced timer
+#endif
+
 	/* toggle the CC output each time the count passes 1 */
 	TONE_rCCR = 1;
 
@@ -428,25 +480,31 @@ ToneAlarm::note_duration(unsigned &silence, unsigned note_length, unsigned dots)
 {
 	unsigned whole_note_period = (60 * 1000000 * 4) / _tempo;
 
-	if (note_length == 0)
+	if (note_length == 0) {
 		note_length = 1;
+	}
+
 	unsigned note_period = whole_note_period / note_length;
 
 	switch (_note_mode) {
 	case MODE_NORMAL:
 		silence = note_period / 8;
 		break;
+
 	case MODE_STACCATO:
 		silence = note_period / 4;
 		break;
+
 	default:
 	case MODE_LEGATO:
 		silence = 0;
 		break;
 	}
+
 	note_period -= silence;
 
 	unsigned dot_extension = note_period / 2;
+
 	while (dots--) {
 		note_period += dot_extension;
 		dot_extension /= 2;
@@ -460,12 +518,14 @@ ToneAlarm::rest_duration(unsigned rest_length, unsigned dots)
 {
 	unsigned whole_note_period = (60 * 1000000 * 4) / _tempo;
 
-	if (rest_length == 0)
+	if (rest_length == 0) {
 		rest_length = 1;
+	}
 
 	unsigned rest_period = whole_note_period / rest_length;
 
 	unsigned dot_extension = rest_period / 2;
+
 	while (dots--) {
 		rest_period += dot_extension;
 		dot_extension /= 2;
@@ -555,115 +615,155 @@ ToneAlarm::next_note()
 	while (note == 0) {
 		// we always need at least one character from the string
 		int c = next_char();
-		if (c == 0)
+
+		if (c == 0) {
 			goto tune_end;
+		}
+
 		_next++;
 
 		switch (c) {
 		case 'L':	// select note length
 			_note_length = next_number();
-			if (_note_length < 1)
+
+			if (_note_length < 1) {
 				goto tune_error;
+			}
+
 			break;
 
 		case 'O':	// select octave
 			_octave = next_number();
-			if (_octave > 6)
+
+			if (_octave > 6) {
 				_octave = 6;
+			}
+
 			break;
 
 		case '<':	// decrease octave
-			if (_octave > 0)
+			if (_octave > 0) {
 				_octave--;
+			}
+
 			break;
 
 		case '>':	// increase octave
-			if (_octave < 6)
+			if (_octave < 6) {
 				_octave++;
+			}
+
 			break;
 
 		case 'M':	// select inter-note gap
 			c = next_char();
-			if (c == 0)
+
+			if (c == 0) {
 				goto tune_error;
+			}
+
 			_next++;
+
 			switch (c) {
 			case 'N':
 				_note_mode = MODE_NORMAL;
 				break;
+
 			case 'L':
 				_note_mode = MODE_LEGATO;
 				break;
+
 			case 'S':
 				_note_mode = MODE_STACCATO;
 				break;
+
 			case 'F':
 				_repeat = false;
 				break;
+
 			case 'B':
 				_repeat = true;
 				break;
+
 			default:
 				goto tune_error;
 			}
+
 			break;
 
 		case 'P':	// pause for a note length
 			stop_note();
-			hrt_call_after(&_note_call, 
-				(hrt_abstime)rest_duration(next_number(), next_dots()),
-				(hrt_callout)next_trampoline, 
-				this);
+			hrt_call_after(&_note_call,
+				       (hrt_abstime)rest_duration(next_number(), next_dots()),
+				       (hrt_callout)next_trampoline,
+				       this);
 			return;
 
 		case 'T': {	// change tempo
-			unsigned nt = next_number();
+				unsigned nt = next_number();
 
-			if ((nt >= 32) && (nt <= 255)) {
-				_tempo = nt;
-			} else {
-				goto tune_error;
+				if ((nt >= 32) && (nt <= 255)) {
+					_tempo = nt;
+
+				} else {
+					goto tune_error;
+				}
+
+				break;
 			}
-			break;
-		}
 
 		case 'N':	// play an arbitrary note
 			note = next_number();
-			if (note > 84)
+
+			if (note > 84) {
 				goto tune_error;
+			}
+
 			if (note == 0) {
 				// this is a rest - pause for the current note length
 				hrt_call_after(&_note_call,
-					(hrt_abstime)rest_duration(_note_length, next_dots()),
-					(hrt_callout)next_trampoline, 
-					this);
-				return;				
+					       (hrt_abstime)rest_duration(_note_length, next_dots()),
+					       (hrt_callout)next_trampoline,
+					       this);
+				return;
 			}
+
 			break;
 
 		case 'A'...'G':	// play a note in the current octave
 			note = _note_tab[c - 'A'] + (_octave * 12) + 1;
 			c = next_char();
+
 			switch (c) {
 			case '#':	// up a semitone
 			case '+':
-				if (note < 84)
+				if (note < 84) {
 					note++;
+				}
+
 				_next++;
 				break;
+
 			case '-':	// down a semitone
-				if (note > 1)
+				if (note > 1) {
 					note--;
+				}
+
 				_next++;
 				break;
+
 			default:
 				// 0 / no next char here is OK
 				break;
 			}
+
 			// shorthand length notation
 			note_length = next_number();
-			if (note_length == 0)
+
+			if (note_length == 0) {
 				note_length = _note_length;
+			}
+
 			break;
 
 		default:
@@ -689,12 +789,15 @@ tune_error:
 	// stop (and potentially restart) the tune
 tune_end:
 	stop_note();
+
 	if (_repeat) {
 		start_tune(_tune);
+
 	} else {
 		_tune = nullptr;
 		_default_tune_number = 0;
 	}
+
 	return;
 }
 
@@ -704,6 +807,7 @@ ToneAlarm::next_char()
 	while (isspace(*_next)) {
 		_next++;
 	}
+
 	return toupper(*_next);
 }
 
@@ -715,8 +819,11 @@ ToneAlarm::next_number()
 
 	for (;;) {
 		c = next_char();
-		if (!isdigit(c))
+
+		if (!isdigit(c)) {
 			return number;
+		}
+
 		_next++;
 		number = (number * 10) + (c - '0');
 	}
@@ -731,6 +838,7 @@ ToneAlarm::next_dots()
 		_next++;
 		dots++;
 	}
+
 	return dots;
 }
 
@@ -764,6 +872,7 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg)
 				_next = nullptr;
 				_repeat = false;
 				_default_tune_number = 0;
+
 			} else {
 				/* always interrupt alarms, unless they are repeating and already playing */
 				if (!(_repeat && _default_tune_number == arg)) {
@@ -772,6 +881,7 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg)
 					start_tune(_default_tunes[arg]);
 				}
 			}
+
 		} else {
 			result = -EINVAL;
 		}
@@ -786,8 +896,9 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg)
 //	irqrestore(flags);
 
 	/* give it to the superclass if we didn't like it */
-	if (result == -ENOTTY)
+	if (result == -ENOTTY) {
 		result = CDev::ioctl(filp, cmd, arg);
+	}
 
 	return result;
 }
@@ -796,8 +907,9 @@ int
 ToneAlarm::write(file *filp, const char *buffer, size_t len)
 {
 	// sanity-check the buffer for length and nul-termination
-	if (len > _tune_max)
+	if (len > _tune_max) {
 		return -EFBIG;
+	}
 
 	// if we have an existing user tune, free it
 	if (_user_tune != nullptr) {
@@ -814,13 +926,16 @@ ToneAlarm::write(file *filp, const char *buffer, size_t len)
 	}
 
 	// if the new tune is empty, we're done
-	if (buffer[0] == '\0')
+	if (buffer[0] == '\0') {
 		return OK;
+	}
 
 	// allocate a copy of the new tune
 	_user_tune = strndup(buffer, len);
-	if (_user_tune == nullptr)
+
+	if (_user_tune == nullptr) {
 		return -ENOMEM;
+	}
 
 	// and play it
 	start_tune(_user_tune);
@@ -843,14 +958,16 @@ play_tune(unsigned tune)
 
 	fd = open(TONEALARM0_DEVICE_PATH, 0);
 
-	if (fd < 0)
+	if (fd < 0) {
 		err(1, TONEALARM0_DEVICE_PATH);
+	}
 
 	ret = ioctl(fd, TONE_SET_ALARM, tune);
 	close(fd);
 
-	if (ret != 0)
+	if (ret != 0) {
 		err(1, "TONE_SET_ALARM");
+	}
 
 	exit(0);
 }
@@ -862,17 +979,21 @@ play_string(const char *str, bool free_buffer)
 
 	fd = open(TONEALARM0_DEVICE_PATH, O_WRONLY);
 
-	if (fd < 0)
+	if (fd < 0) {
 		err(1, TONEALARM0_DEVICE_PATH);
+	}
 
 	ret = write(fd, str, strlen(str) + 1);
 	close(fd);
 
-	if (free_buffer)
+	if (free_buffer) {
 		free((void *)str);
+	}
 
-	if (ret < 0)
+	if (ret < 0) {
 		err(1, "play tune");
+	}
+
 	exit(0);
 }
 
@@ -887,8 +1008,9 @@ tone_alarm_main(int argc, char *argv[])
 	if (g_dev == nullptr) {
 		g_dev = new ToneAlarm;
 
-		if (g_dev == nullptr)
+		if (g_dev == nullptr) {
 			errx(1, "couldn't allocate the ToneAlarm driver");
+		}
 
 		if (g_dev->init() != OK) {
 			delete g_dev;
@@ -903,30 +1025,39 @@ tone_alarm_main(int argc, char *argv[])
 			play_tune(TONE_STOP_TUNE);
 		}
 
-		if (!strcmp(argv1, "stop"))
+		if (!strcmp(argv1, "stop")) {
 			play_tune(TONE_STOP_TUNE);
+		}
 
-		if ((tune = strtol(argv1, nullptr, 10)) != 0)
+		if ((tune = strtol(argv1, nullptr, 10)) != 0) {
 			play_tune(tune);
+		}
 
 		/* It might be a tune name */
 		for (tune = 1; tune < TONE_NUMBER_OF_TUNES; tune++)
-			if (!strcmp(g_dev->name(tune), argv1))
+			if (!strcmp(g_dev->name(tune), argv1)) {
 				play_tune(tune);
+			}
 
 		/* If it is a file name then load and play it as a string */
 		if (*argv1 == '/') {
 			FILE *fd = fopen(argv1, "r");
 			int sz;
 			char *buffer;
-			if (fd == nullptr)
+
+			if (fd == nullptr) {
 				errx(1, "couldn't open '%s'", argv1);
+			}
+
 			fseek(fd, 0, SEEK_END);
 			sz = ftell(fd);
 			fseek(fd, 0, SEEK_SET);
 			buffer = (char *)malloc(sz + 1);
-			if (buffer == nullptr)
+
+			if (buffer == nullptr) {
 				errx(1, "not enough memory memory");
+			}
+
 			fread(buffer, sz, 1, fd);
 			/* terminate the string */
 			buffer[sz] = 0;

src/examples/fixedwing_control/main.c

@@ -432,11 +432,11 @@ int ex_fixedwing_control_main(int argc, char *argv[])
 
 		thread_should_exit = false;
 		deamon_task = px4_task_spawn_cmd("ex_fixedwing_control",
-					     SCHED_DEFAULT,
-					     SCHED_PRIORITY_MAX - 20,
-					     2048,
-					     fixedwing_control_thread_main,
-					     (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
+						 SCHED_DEFAULT,
+						 SCHED_PRIORITY_MAX - 20,
+						 2048,
+						 fixedwing_control_thread_main,
+						 (argv) ? (char *const *)&argv[2] : (char *const *)NULL);
 		thread_running = true;
 		exit(0);
 	}

src/examples/flow_position_estimator/flow_position_estimator_main.c

@@ -112,11 +112,11 @@ int flow_position_estimator_main(int argc, char *argv[])
 
 		thread_should_exit = false;
 		daemon_task = px4_task_spawn_cmd("flow_position_estimator",
-					     SCHED_DEFAULT,
-					     SCHED_PRIORITY_MAX - 5,
-					     4000,
-					     flow_position_estimator_thread_main,
-					     (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
+						 SCHED_DEFAULT,
+						 SCHED_PRIORITY_MAX - 5,
+						 4000,
+						 flow_position_estimator_thread_main,
+						 (argv) ? (char *const *)&argv[2] : (char *const *)NULL);
 		exit(0);
 	}
 

src/examples/matlab_csv_serial/matlab_csv_serial.c

@@ -104,11 +104,11 @@ int matlab_csv_serial_main(int argc, char *argv[])
 
 		thread_should_exit = false;
 		daemon_task = px4_task_spawn_cmd("matlab_csv_serial",
-					     SCHED_DEFAULT,
-					     SCHED_PRIORITY_MAX - 5,
-					     2000,
-					     matlab_csv_serial_thread_main,
-					     (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
+						 SCHED_DEFAULT,
+						 SCHED_PRIORITY_MAX - 5,
+						 2000,
+						 matlab_csv_serial_thread_main,
+						 (argv) ? (char *const *)&argv[2] : (char *const *)NULL);
 		exit(0);
 	}
 

src/examples/publisher/publisher_start_nuttx.cpp

@@ -69,11 +69,11 @@ int publisher_main(int argc, char *argv[])
 		task_should_exit = false;
 
 		daemon_task = px4_task_spawn_cmd("publisher",
-				       SCHED_DEFAULT,
-				       SCHED_PRIORITY_MAX - 5,
-				       2000,
-				       main,
-					(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
+						 SCHED_DEFAULT,
+						 SCHED_PRIORITY_MAX - 5,
+						 2000,
+						 main,
+						 (argv) ? (char *const *)&argv[2] : (char *const *)NULL);
 
 		exit(0);
 	}

src/examples/px4_daemon_app/px4_daemon_app.c

@@ -103,11 +103,11 @@ int px4_daemon_app_main(int argc, char *argv[])
 
 		thread_should_exit = false;
 		daemon_task = px4_task_spawn_cmd("daemon",
-					     SCHED_DEFAULT,
-					     SCHED_PRIORITY_DEFAULT,
-					     2000,
-					     px4_daemon_thread_main,
-					     (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
+						 SCHED_DEFAULT,
+						 SCHED_PRIORITY_DEFAULT,
+						 2000,
+						 px4_daemon_thread_main,
+						 (argv) ? (char *const *)&argv[2] : (char *const *)NULL);
 		return 0;
 	}
 

src/examples/rover_steering_control/main.cpp

@@ -426,11 +426,11 @@ int rover_steering_control_main(int argc, char *argv[])
 
 		thread_should_exit = false;
 		deamon_task = px4_task_spawn_cmd("rover_steering_control",
-					     SCHED_DEFAULT,
-					     SCHED_PRIORITY_MAX - 20,
-					     2048,
-					     rover_steering_control_thread_main,
-					     (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
+						 SCHED_DEFAULT,
+						 SCHED_PRIORITY_MAX - 20,
+						 2048,
+						 rover_steering_control_thread_main,
+						 (argv) ? (char *const *)&argv[2] : (char *const *)NULL);
 		thread_running = true;
 		exit(0);
 	}

src/examples/subscriber/subscriber_start_nuttx.cpp

@@ -69,11 +69,11 @@ int subscriber_main(int argc, char *argv[])
 		task_should_exit = false;
 
 		daemon_task = px4_task_spawn_cmd("subscriber",
-				       SCHED_DEFAULT,
-				       SCHED_PRIORITY_MAX - 5,
-				       2000,
-				       main,
-					(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
+						 SCHED_DEFAULT,
+						 SCHED_PRIORITY_MAX - 5,
+						 2000,
+						 main,
+						 (argv) ? (char *const *)&argv[2] : (char *const *)NULL);
 
 		exit(0);
 	}

src/include/containers/List.hpp

@@ -43,31 +43,35 @@ template<class T>
 class __EXPORT ListNode
 {
 public:
-	ListNode() : _sibling(nullptr) {
+	ListNode() : _sibling(nullptr)
+	{
 	}
 	virtual ~ListNode() {};
 	void setSibling(T sibling) { _sibling = sibling; }
 	T getSibling() { return _sibling; }
-	T get() {
+	T get()
+	{
 		return _sibling;
 	}
 protected:
 	T _sibling;
 private:
 	// forbid copy
-	ListNode(const ListNode& other);
+	ListNode(const ListNode &other);
 	// forbid assignment
-	ListNode & operator = (const ListNode &);
+	ListNode &operator = (const ListNode &);
 };
 
 template<class T>
 class __EXPORT List
 {
 public:
-	List() : _head() {
+	List() : _head()
+	{
 	}
 	virtual ~List() {};
-	void add(T newNode) {
+	void add(T newNode)
+	{
 		newNode->setSibling(getHead());
 		setHead(newNode);
 	}
@@ -77,7 +81,7 @@ protected:
 	T _head;
 private:
 	// forbid copy
-	List(const List& other);
+	List(const List &other);
 	// forbid assignment
-	List& operator = (const List &);
+	List &operator = (const List &);
 };

src/include/mavlink/mavlink_log.h

@@ -107,9 +107,9 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...);
  * @param _text		The text to log;
  */
 #define mavlink_and_console_log_emergency(_fd, _text, ...)		mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__); \
-									fprintf(stderr, "telem> "); \
-									fprintf(stderr, _text, ##__VA_ARGS__); \
-									fprintf(stderr, "\n");
+	fprintf(stderr, "telem> "); \
+	fprintf(stderr, _text, ##__VA_ARGS__); \
+	fprintf(stderr, "\n");
 
 /**
  * Send a mavlink critical message and print to console.
@@ -118,9 +118,9 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...);
  * @param _text		The text to log;
  */
 #define mavlink_and_console_log_critical(_fd, _text, ...)		mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__); \
-									fprintf(stderr, "telem> "); \
-									fprintf(stderr, _text, ##__VA_ARGS__); \
-									fprintf(stderr, "\n");
+	fprintf(stderr, "telem> "); \
+	fprintf(stderr, _text, ##__VA_ARGS__); \
+	fprintf(stderr, "\n");
 
 /**
  * Send a mavlink emergency message and print to console.
@@ -129,9 +129,9 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...);
  * @param _text		The text to log;
  */
 #define mavlink_and_console_log_info(_fd, _text, ...)			mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); \
-									fprintf(stderr, "telem> "); \
-									fprintf(stderr, _text, ##__VA_ARGS__); \
-									fprintf(stderr, "\n");
+	fprintf(stderr, "telem> "); \
+	fprintf(stderr, _text, ##__VA_ARGS__); \
+	fprintf(stderr, "\n");
 
 struct mavlink_logmessage {
 	char text[MAVLINK_LOG_MAXLEN + 1];

src/lib/ecl/ecl.h

@@ -38,6 +38,9 @@
  */
 
 #include <drivers/drv_hrt.h>
+#include <px4_log.h>
 
 #define ecl_absolute_time hrt_absolute_time
 #define ecl_elapsed_time hrt_elapsed_time
+#define ECL_WARN PX4_WARN
+#define ECL_INFO PX4_INFO

src/lib/ecl/module.mk

@@ -39,6 +39,8 @@ SRCS		 = 	attitude_fw/ecl_controller.cpp \
 			attitude_fw/ecl_pitch_controller.cpp \
 			attitude_fw/ecl_roll_controller.cpp \
 			attitude_fw/ecl_yaw_controller.cpp \
-			l1/ecl_l1_pos_controller.cpp
+			l1/ecl_l1_pos_controller.cpp \
+			validation/data_validator.cpp \
+			validation/data_validator_group.cpp
 
 MAXOPTIMIZATION	 = -Os

src/lib/ecl/validation/data_validator.cpp

@@ -0,0 +1,162 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 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 data_validator.c
+ *
+ * A data validation class to identify anomalies in data streams
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ */
+
+#include "data_validator.h"
+#include <ecl/ecl.h>
+
+DataValidator::DataValidator(DataValidator *prev_sibling) :
+	_time_last(0),
+	_timeout_interval(20000),
+	_event_count(0),
+	_error_count(0),
+	_error_density(0),
+	_priority(0),
+	_mean{0.0f},
+	_lp{0.0f},
+	_M2{0.0f},
+	_rms{0.0f},
+	_value{0.0f},
+	_value_equal_count(0),
+	_sibling(prev_sibling)
+{
+
+}
+
+DataValidator::~DataValidator()
+{
+
+}
+
+void
+DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in, int priority_in)
+{
+	_event_count++;
+
+	if (error_count_in > _error_count) {
+		_error_density += (error_count_in - _error_count);
+	} else if (_error_density > 0) {
+		_error_density--;
+	}
+
+	_error_count = error_count_in;
+	_priority = priority_in;
+
+	for (unsigned i = 0; i < _dimensions; i++) {
+		if (_time_last == 0) {
+			_mean[i] = 0;
+			_lp[i] = val[i];
+			_M2[i] = 0;
+		} else {
+			float lp_val = val[i] - _lp[i];
+
+			float delta_val = lp_val - _mean[i];
+			_mean[i] += delta_val / _event_count;
+			_M2[i] += delta_val * (lp_val - _mean[i]);
+			_rms[i] = sqrtf(_M2[i] / (_event_count - 1));
+
+			if (fabsf(_value[i] - val[i]) < 0.000001f) {
+				_value_equal_count++;
+			} else {
+				_value_equal_count = 0;
+			}
+		}
+
+		// XXX replace with better filter, make it auto-tune to update rate
+		_lp[i] = _lp[i] * 0.5f + val[i] * 0.5f;
+
+		_value[i] = val[i];
+	}
+
+	_time_last = timestamp;
+}
+
+float
+DataValidator::confidence(uint64_t timestamp)
+{
+	/* check if we have any data */
+	if (_time_last == 0) {
+		return 0.0f;
+	}
+
+	/* check error count limit */
+	if (_error_count > NORETURN_ERRCOUNT) {
+		return 0.0f;
+	}
+
+	/* we got the exact same sensor value N times in a row */
+	if (_value_equal_count > VALUE_EQUAL_COUNT_MAX) {
+		return 0.0f;
+	}
+
+	/* timed out - that's it */
+	if (timestamp - _time_last > _timeout_interval) {
+		return 0.0f;
+	}
+
+	/* cap error density counter at window size */
+	if (_error_density > ERROR_DENSITY_WINDOW) {
+		_error_density = ERROR_DENSITY_WINDOW;
+	}
+
+	/* return local error density for last N measurements */
+	return 1.0f - (_error_density / ERROR_DENSITY_WINDOW);
+}
+
+int
+DataValidator::priority()
+{
+	return _priority;
+}
+
+void
+DataValidator::print()
+{
+	if (_time_last == 0) {
+		ECL_INFO("\tno data");
+		return;
+	}
+
+	for (unsigned i = 0; i < _dimensions; i++) {
+		ECL_INFO("\tval: %8.4f, lp: %8.4f mean dev: %8.4f RMS: %8.4f conf: %8.4f",
+			(double) _value[i], (double)_lp[i], (double)_mean[i],
+			(double)_rms[i], (double)confidence(hrt_absolute_time()));
+	}
+}

src/lib/ecl/validation/data_validator.h

@@ -0,0 +1,137 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 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 data_validator.h
+ *
+ * A data validation class to identify anomalies in data streams
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ */
+
+#pragma once
+
+#include <cmath>
+#include <stdint.h>
+
+class __EXPORT DataValidator {
+public:
+	DataValidator(DataValidator *prev_sibling = nullptr);
+	virtual ~DataValidator();
+
+	/**
+	 * Put an item into the validator.
+	 *
+	 * @param val		Item to put
+	 */
+	void			put(uint64_t timestamp, float val[3], uint64_t error_count, int priority);
+
+	/**
+	 * Get the next sibling in the group
+	 *
+	 * @return		the next sibling
+	 */
+	DataValidator*		sibling() { return _sibling; }
+
+	/**
+	 * Get the confidence of this validator
+	 * @return		the confidence between 0 and 1
+	 */
+	float			confidence(uint64_t timestamp);
+
+	/**
+	 * Get the error count of this validator
+	 * @return		the error count
+	 */
+	uint64_t		error_count() { return _error_count; }
+
+	/**
+	 * Get the values of this validator
+	 * @return		the stored value
+	 */
+	float*			value() { return _value; }
+
+	/**
+	 * Get the used status of this validator
+	 * @return		true if this validator ever saw data
+	 */
+	bool			used() { return (_time_last > 0); }
+
+	/**
+	 * Get the priority of this validator
+	 * @return		the stored priority
+	 */
+	int			priority();
+
+	/**
+	 * Get the RMS values of this validator
+	 * @return		the stored RMS
+	 */
+	float*			rms() { return _rms; }
+
+	/**
+	 * Print the validator value
+	 *
+	 */
+	void			print();
+
+	/**
+	 * Set the timeout value
+	 *
+	 * @param timeout_interval_us The timeout interval in microseconds
+	 */
+	void			set_timeout(uint64_t timeout_interval_us) { _timeout_interval = timeout_interval_us; }
+
+private:
+	static const unsigned _dimensions = 3;
+	uint64_t _time_last;			/**< last timestamp */
+	uint64_t _timeout_interval;		/**< interval in which the datastream times out in us */
+	uint64_t _event_count;			/**< total data counter */
+	uint64_t _error_count;			/**< error count */
+	int _error_density;			/**< ratio between successful reads and errors */
+	int _priority;				/**< sensor nominal priority */
+	float _mean[_dimensions];		/**< mean of value */
+	float _lp[3];				/**< low pass value */
+	float _M2[3];				/**< RMS component value */
+	float _rms[3];				/**< root mean square error */
+	float _value[3];			/**< last value */
+	float _value_equal_count;		/**< equal values in a row */
+	DataValidator *_sibling;		/**< sibling in the group */
+	const unsigned NORETURN_ERRCOUNT = 10000;	/**< if the error count reaches this value, return sensor as invalid */
+	const float ERROR_DENSITY_WINDOW = 100.0f; /**< window in measurement counts for errors */
+	const unsigned VALUE_EQUAL_COUNT_MAX = 100;	/**< if the sensor value is the same (accumulated also between axes) this many times, flag it */
+
+	/* we don't want this class to be copied */
+	DataValidator(const DataValidator&);
+	DataValidator operator=(const DataValidator&);
+};

src/lib/ecl/validation/data_validator_group.cpp

@@ -0,0 +1,224 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 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 data_validator_group.cpp
+ *
+ * A data validation group to identify anomalies in data streams
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ */
+
+#include "data_validator_group.h"
+#include <ecl/ecl.h>
+
+DataValidatorGroup::DataValidatorGroup(unsigned siblings) :
+	_first(nullptr),
+	_curr_best(-1),
+	_prev_best(-1),
+	_first_failover_time(0),
+	_toggle_count(0)
+{
+	DataValidator *next = _first;
+
+	for (unsigned i = 0; i < siblings; i++) {
+		next = new DataValidator(next);
+	}
+
+	_first = next;
+}
+
+DataValidatorGroup::~DataValidatorGroup()
+{
+
+}
+
+void
+DataValidatorGroup::set_timeout(uint64_t timeout_interval_us)
+{
+	DataValidator *next = _first;
+
+	while (next != nullptr) {
+		next->set_timeout(timeout_interval_us);
+		next = next->sibling();
+	}
+}
+
+void
+DataValidatorGroup::put(unsigned index, uint64_t timestamp, float val[3], uint64_t error_count, int priority)
+{
+	DataValidator *next = _first;
+	unsigned i = 0;
+
+	while (next != nullptr) {
+		if (i == index) {
+			next->put(timestamp, val, error_count, priority);
+			break;
+		}
+		next = next->sibling();
+		i++;
+	}
+}
+
+float*
+DataValidatorGroup::get_best(uint64_t timestamp, int *index)
+{
+	DataValidator *next = _first;
+
+	// XXX This should eventually also include voting
+	int pre_check_best = _curr_best;
+	float pre_check_confidence = 1.0f;
+	int pre_check_prio = -1;
+	float max_confidence = -1.0f;
+	int max_priority = -1000;
+	int max_index = -1;
+	DataValidator *best = nullptr;
+
+	unsigned i = 0;
+
+	while (next != nullptr) {
+		float confidence = next->confidence(timestamp);
+
+		if (static_cast<int>(i) == pre_check_best) {
+			pre_check_prio = next->priority();
+			pre_check_confidence = confidence;
+		}
+
+		/*
+		 * Switch if:
+		 * 1) the confidence is higher and priority is equal or higher
+		 * 2) the confidence is no less than 1% different and the priority is higher
+		 */
+		if (((max_confidence < MIN_REGULAR_CONFIDENCE) && (confidence >= MIN_REGULAR_CONFIDENCE)) ||
+			(confidence > max_confidence && (next->priority() >= max_priority)) ||
+			(fabsf(confidence - max_confidence) < 0.01f && (next->priority() > max_priority))
+			) {
+			max_index = i;
+			max_confidence = confidence;
+			max_priority = next->priority();
+			best = next;
+		}
+
+		next = next->sibling();
+		i++;
+	}
+
+	/* the current best sensor is not matching the previous best sensor */
+	if (max_index != _curr_best) {
+
+		bool true_failsafe = true;
+
+		/* check wether the switch was a failsafe or preferring a higher priority sensor */
+		if (pre_check_prio != -1 && pre_check_prio < max_priority &&
+			fabsf(pre_check_confidence - max_confidence) < 0.1f) {
+			/* this is not a failover */
+			true_failsafe = false;
+		}
+
+		/* if we're no initialized, initialize the bookkeeping but do not count a failsafe */
+		if (_curr_best < 0) {
+			_prev_best = max_index;
+		} else {
+			/* we were initialized before, this is a real failsafe */
+			_prev_best = pre_check_best;
+
+			if (true_failsafe) {
+				_toggle_count++;
+
+				/* if this is the first time, log when we failed */
+				if (_first_failover_time == 0) {
+					_first_failover_time = timestamp;
+				}
+			}
+		}
+
+		/* for all cases we want to keep a record of the best index */
+		_curr_best = max_index;
+	}
+	*index = max_index;
+	return (best) ? best->value() : nullptr;
+}
+
+float
+DataValidatorGroup::get_vibration_factor(uint64_t timestamp)
+{
+	DataValidator *next = _first;
+
+	float vibe = 0.0f;
+
+	/* find the best RMS value of a non-timed out sensor */
+	while (next != nullptr) {
+
+		if (next->confidence(timestamp) > 0.5f) {
+			float* rms = next->rms();
+
+			for (unsigned j = 0; j < 3; j++) {
+				if (rms[j] > vibe) {
+					vibe = rms[j];
+				}
+			}
+		}
+
+		next = next->sibling();
+	}
+
+	return vibe;
+}
+
+void
+DataValidatorGroup::print()
+{
+	/* print the group's state */
+	ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (# %u)",
+		_curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO",
+		_toggle_count);
+
+
+	DataValidator *next = _first;
+	unsigned i = 0;
+
+	while (next != nullptr) {
+		if (next->used()) {
+			ECL_INFO("sensor #%u, prio: %d", i, next->priority());
+			next->print();
+		}
+		next = next->sibling();
+		i++;
+	}
+}
+
+unsigned
+DataValidatorGroup::failover_count()
+{
+	return _toggle_count;
+}

src/lib/ecl/validation/data_validator_group.h

@@ -0,0 +1,108 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 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 data_validator_group.h
+ *
+ * A data validation group to identify anomalies in data streams
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ */
+
+#pragma once
+
+#include "data_validator.h"
+
+class __EXPORT DataValidatorGroup {
+public:
+	DataValidatorGroup(unsigned siblings);
+	virtual ~DataValidatorGroup();
+
+	/**
+	 * Put an item into the validator group.
+	 *
+	 * @param index		Sensor index
+	 * @param timestamp	The timestamp of the measurement
+	 * @param val		The 3D vector
+	 * @param error_count	The current error count of the sensor
+	 * @param priority	The priority of the sensor
+	 */
+	void			put(unsigned index, uint64_t timestamp,
+					float val[3], uint64_t error_count, int priority);
+
+	/**
+	 * Get the best data triplet of the group
+	 *
+	 * @return		pointer to the array of best values
+	 */
+	float*			get_best(uint64_t timestamp, int *index);
+
+	/**
+	 * Get the RMS / vibration factor
+	 *
+	 * @return		float value representing the RMS, which a valid indicator for vibration
+	 */
+	float			get_vibration_factor(uint64_t timestamp);
+
+	/**
+	 * Get the number of failover events
+	 *
+	 * @return		the number of failovers
+	 */
+	unsigned		failover_count();
+
+	/**
+	 * Print the validator value
+	 *
+	 */
+	void			print();
+
+	/**
+	 * Set the timeout value
+	 *
+	 * @param timeout_interval_us The timeout interval in microseconds
+	 */
+	void			set_timeout(uint64_t timeout_interval_us);
+
+private:
+	DataValidator *_first;		/**< sibling in the group */
+	int _curr_best;		/**< currently best index */
+	int _prev_best;		/**< the previous best index */
+	uint64_t _first_failover_time;	/**< timestamp where the first failover occured or zero if none occured */
+	unsigned _toggle_count;		/**< number of back and forth switches between two sensors */
+	static constexpr float MIN_REGULAR_CONFIDENCE = 0.9f;
+
+	/* we don't want this class to be copied */
+	DataValidatorGroup(const DataValidatorGroup&);
+	DataValidatorGroup operator=(const DataValidatorGroup&);
+};

src/lib/launchdetection/CatapultLaunchMethod.cpp

@@ -58,7 +58,8 @@ CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) :
 
 }
 
-CatapultLaunchMethod::~CatapultLaunchMethod() {
+CatapultLaunchMethod::~CatapultLaunchMethod()
+{
 
 }
 
@@ -69,34 +70,41 @@ void CatapultLaunchMethod::update(float accel_x)
 
 	switch (state) {
 	case LAUNCHDETECTION_RES_NONE:
+
 		/* Detect a acceleration that is longer and stronger as the minimum given by the params */
 		if (accel_x > thresholdAccel.get()) {
 			integrator += dt;
+
 			if (integrator > thresholdTime.get()) {
 				if (motorDelay.get() > 0.0f) {
 					state = LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL;
 					warnx("Launch detected: state: enablecontrol, waiting %.2fs until using full"
-							" throttle", (double)motorDelay.get());
+					      " throttle", (double)motorDelay.get());
+
 				} else {
 					/* No motor delay set: go directly to enablemotors state */
 					state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
 					warnx("Launch detected: state: enablemotors (delay not activated)");
 				}
 			}
+
 		} else {
 			/* reset */
 			reset();
 		}
+
 		break;
 
 	case LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL:
-		/* Vehicle is currently controlling attitude but not with full throttle. Waiting undtil delay is
+		/* Vehicle is currently controlling attitude but not with full throttle. Waiting until delay is
 		 * over to allow full throttle */
 		motorDelayCounter += dt;
+
 		if (motorDelayCounter > motorDelay.get()) {
 			warnx("Launch detected: state enablemotors");
 			state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
 		}
+
 		break;
 
 	default:
@@ -119,10 +127,12 @@ void CatapultLaunchMethod::reset()
 	state = LAUNCHDETECTION_RES_NONE;
 }
 
-float CatapultLaunchMethod::getPitchMax(float pitchMaxDefault) {
+float CatapultLaunchMethod::getPitchMax(float pitchMaxDefault)
+{
 	/* If motor is turned on do not impose the extra limit on maximum pitch */
 	if (state == LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
 		return pitchMaxDefault;
+
 	} else {
 		return pitchMaxPreThrottle.get();
 	}

src/lib/launchdetection/LaunchDetector.cpp

@@ -66,7 +66,7 @@ LaunchDetector::~LaunchDetector()
 void LaunchDetector::reset()
 {
 	/* Reset all detectors */
-	for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
+	for (unsigned i = 0; i < (sizeof(launchMethods) / sizeof(launchMethods[0])); i++) {
 		launchMethods[i]->reset();
 	}
 
@@ -79,7 +79,7 @@ void LaunchDetector::reset()
 void LaunchDetector::update(float accel_x)
 {
 	if (launchdetection_on.get() == 1) {
-		for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
+		for (unsigned i = 0; i < (sizeof(launchMethods) / sizeof(launchMethods[0])); i++) {
 			launchMethods[i]->update(accel_x);
 		}
 	}
@@ -89,14 +89,15 @@ LaunchDetectionResult LaunchDetector::getLaunchDetected()
 {
 	if (launchdetection_on.get() == 1) {
 		if (activeLaunchDetectionMethodIndex < 0) {
-		/* None of the active launchmethods has detected a launch, check all launchmethods */
-			for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
-				if(launchMethods[i]->getLaunchDetected() != LAUNCHDETECTION_RES_NONE) {
+			/* None of the active launchmethods has detected a launch, check all launchmethods */
+			for (unsigned i = 0; i < (sizeof(launchMethods) / sizeof(launchMethods[0])); i++) {
+				if (launchMethods[i]->getLaunchDetected() != LAUNCHDETECTION_RES_NONE) {
 					warnx("selecting launchmethod %d", i);
 					activeLaunchDetectionMethodIndex = i; // from now on only check this method
 					return launchMethods[i]->getLaunchDetected();
 				}
 			}
+
 		} else {
 			return launchMethods[activeLaunchDetectionMethodIndex]->getLaunchDetected();
 		}
@@ -105,7 +106,8 @@ LaunchDetectionResult LaunchDetector::getLaunchDetected()
 	return LAUNCHDETECTION_RES_NONE;
 }
 
-float LaunchDetector::getPitchMax(float pitchMaxDefault) {
+float LaunchDetector::getPitchMax(float pitchMaxDefault)
+{
 	if (!launchdetection_on.get()) {
 		return pitchMaxDefault;
 	}
@@ -113,11 +115,13 @@ float LaunchDetector::getPitchMax(float pitchMaxDefault) {
 	/* if a lauchdetectionmethod is active or only one exists return the pitch limit from this method,
 	 * otherwise use the default limit */
 	if (activeLaunchDetectionMethodIndex < 0) {
-		if (sizeof(launchMethods)/sizeof(LaunchMethod) > 1) {
+		if (sizeof(launchMethods) / sizeof(LaunchMethod *) > 1) {
 			return pitchMaxDefault;
+
 		} else {
 			return launchMethods[0]->getPitchMax(pitchMaxDefault);
 		}
+
 	} else {
 		return launchMethods[activeLaunchDetectionMethodIndex]->getPitchMax(pitchMaxDefault);
 	}

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp

@@ -383,10 +383,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					uint8_t update_vect[3] = {0, 0, 0};
 
 					/* Fill in gyro measurements */
-					if (sensor_last_timestamp[0] != raw.timestamp) {
+					if (sensor_last_timestamp[0] != raw.gyro_timestamp[0]) {
 						update_vect[0] = 1;
 						// sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
-						sensor_last_timestamp[0] = raw.timestamp;
+						sensor_last_timestamp[0] = raw.gyro_timestamp[0];
 					}
 
 					z_k[0] =  raw.gyro_rad_s[0] - gyro_offsets[0];
@@ -394,10 +394,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					z_k[2] =  raw.gyro_rad_s[2] - gyro_offsets[2];
 
 					/* update accelerometer measurements */
-					if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
+					if (sensor_last_timestamp[1] != raw.accelerometer_timestamp[0]) {
 						update_vect[1] = 1;
 						// sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
-						sensor_last_timestamp[1] = raw.accelerometer_timestamp;
+						sensor_last_timestamp[1] = raw.accelerometer_timestamp[0];
 					}
 
 					hrt_abstime vel_t = 0;
@@ -437,14 +437,14 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					z_k[5] = raw.accelerometer_m_s2[2] - acc(2);
 
 					/* update magnetometer measurements */
-					if (sensor_last_timestamp[2] != raw.magnetometer_timestamp &&
+					if (sensor_last_timestamp[2] != raw.magnetometer_timestamp[0] &&
 						/* check that the mag vector is > 0 */
 						fabsf(sqrtf(raw.magnetometer_ga[0] * raw.magnetometer_ga[0] +
 							raw.magnetometer_ga[1] * raw.magnetometer_ga[1] +
 							raw.magnetometer_ga[2] * raw.magnetometer_ga[2])) > 0.1f) {
 						update_vect[2] = 1;
 						// sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
-						sensor_last_timestamp[2] = raw.magnetometer_timestamp;
+						sensor_last_timestamp[2] = raw.magnetometer_timestamp[0];
 					}
 
 					bool vision_updated = false;

src/modules/attitude_estimator_ekf/module.mk

@@ -46,5 +46,5 @@ MODULE_STACKSIZE = 1200
 EXTRACFLAGS = -Wno-float-equal -Wframe-larger-than=3700
 
 ifeq ($(PX4_TARGET_OS),nuttx)
-EXTRACXXFLAGS = -Wframe-larger-than=2400
+EXTRACXXFLAGS = -Wframe-larger-than=2600
 endif

src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

@@ -51,7 +51,6 @@
 #include <limits.h>
 #include <math.h>
 #include <uORB/uORB.h>
-#include <uORB/topics/debug_key_value.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_control_mode.h>
@@ -59,8 +58,11 @@
 #include <uORB/topics/parameter_update.h>
 #include <drivers/drv_hrt.h>
 
-#include <lib/mathlib/mathlib.h>
+#include <mathlib/mathlib.h>
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
 #include <lib/geo/geo.h>
+#include <lib/ecl/validation/data_validator_group.h>
+#include <mavlink/mavlink_log.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
@@ -103,6 +105,8 @@ public:
 
 	void		task_main();
 
+	void		print();
+
 private:
 	static constexpr float _dt_max = 0.02;
 	bool		_task_should_exit = false;		/**< if true, task should exit */
@@ -121,6 +125,7 @@ private:
 		param_t	mag_decl_auto;
 		param_t	acc_comp;
 		param_t	bias_max;
+		param_t vibe_thresh;
 	}		_params_handles;		/**< handles for interesting parameters */
 
 	float		_w_accel = 0.0f;
@@ -130,6 +135,8 @@ private:
 	bool		_mag_decl_auto = false;
 	bool		_acc_comp = false;
 	float		_bias_max = 0.0f;
+	float		_vibration_warning_threshold = 1.0f;
+	hrt_abstime	_vibration_warning_timestamp = 0;
 
 	Vector<3>	_gyro;
 	Vector<3>	_accel;
@@ -142,10 +149,23 @@ private:
 	vehicle_global_position_s _gpos = {};
 	Vector<3>	_vel_prev;
 	Vector<3>	_pos_acc;
+
+	DataValidatorGroup _voter_gyro;
+	DataValidatorGroup _voter_accel;
+	DataValidatorGroup _voter_mag;
+
+	/* Low pass filter for attitude rates */
+	math::LowPassFilter2p _lp_roll_rate;
+	math::LowPassFilter2p _lp_pitch_rate;
+
 	hrt_abstime _vel_prev_t = 0;
 
 	bool		_inited = false;
 	bool		_data_good = false;
+	bool		_failsafe = false;
+	bool		_vibration_warning = false;
+
+	int		_mavlink_fd = -1;
 
 	perf_counter_t _update_perf;
 	perf_counter_t _loop_perf;
@@ -160,7 +180,15 @@ private:
 };
 
 
-AttitudeEstimatorQ::AttitudeEstimatorQ() {
+AttitudeEstimatorQ::AttitudeEstimatorQ() :
+	_voter_gyro(3),
+	_voter_accel(3),
+	_voter_mag(3),
+	_lp_roll_rate(250.0f, 20.0f),
+	_lp_pitch_rate(250.0f, 20.0f)
+{
+	_voter_mag.set_timeout(200000);
+	
 	_params_handles.w_acc		= param_find("ATT_W_ACC");
 	_params_handles.w_mag		= param_find("ATT_W_MAG");
 	_params_handles.w_gyro_bias	= param_find("ATT_W_GYRO_BIAS");
@@ -168,12 +196,14 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() {
 	_params_handles.mag_decl_auto	= param_find("ATT_MAG_DECL_A");
 	_params_handles.acc_comp	= param_find("ATT_ACC_COMP");
 	_params_handles.bias_max	= param_find("ATT_BIAS_MAX");
+	_params_handles.vibe_thresh	= param_find("ATT_VIBE_THRESH");
 }
 
 /**
  * Destructor, also kills task.
  */
-AttitudeEstimatorQ::~AttitudeEstimatorQ() {
+AttitudeEstimatorQ::~AttitudeEstimatorQ()
+{
 	if (_control_task != -1) {
 		/* task wakes up every 100ms or so at the longest */
 		_task_should_exit = true;
@@ -196,14 +226,15 @@ AttitudeEstimatorQ::~AttitudeEstimatorQ() {
 	attitude_estimator_q::instance = nullptr;
 }
 
-int AttitudeEstimatorQ::start() {
+int AttitudeEstimatorQ::start()
+{
 	ASSERT(_control_task == -1);
 
 	/* start the task */
 	_control_task = px4_task_spawn_cmd("attitude_estimator_q",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
-				       2000,
+				       2100,
 				       (px4_main_t)&AttitudeEstimatorQ::task_main_trampoline,
 				       nullptr);
 
@@ -215,12 +246,23 @@ int AttitudeEstimatorQ::start() {
 	return OK;
 }
 
-void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[]) {
+void AttitudeEstimatorQ::print()
+{
+	warnx("gyro status:");
+	_voter_gyro.print();
+	warnx("accel status:");
+	_voter_accel.print();
+	warnx("mag status:");
+	_voter_mag.print();
+}
+
+void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[])
+{
 	attitude_estimator_q::instance->task_main();
 }
 
-void AttitudeEstimatorQ::task_main() {
-
+void AttitudeEstimatorQ::task_main()
+{
 	_sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
@@ -236,6 +278,10 @@ void AttitudeEstimatorQ::task_main() {
 	while (!_task_should_exit) {
 		int ret = px4_poll(fds, 1, 1000);
 
+		if (_mavlink_fd < 0) {
+			_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+		}
+
 		if (ret < 0) {
 			// Poll error, sleep and try again
 			usleep(10000);
@@ -250,11 +296,71 @@ void AttitudeEstimatorQ::task_main() {
 		// Update sensors
 		sensor_combined_s sensors;
 		if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) {
-			_gyro.set(sensors.gyro_rad_s);
-			_accel.set(sensors.accelerometer_m_s2);
-			_mag.set(sensors.magnetometer_ga);
+			// Feed validator with recent sensor data
+
+			for (unsigned i = 0; i < (sizeof(sensors.gyro_timestamp) / sizeof(sensors.gyro_timestamp[0])); i++) {
+
+				/* ignore empty fields */
+				if (sensors.gyro_timestamp[i] > 0) {
+
+					float gyro[3];
+
+					for (unsigned j = 0; j < 3; j++) {
+						if (sensors.gyro_integral_dt[i] > 0) {
+							gyro[j] = (double)sensors.gyro_integral_rad[i * 3 + j] / (sensors.gyro_integral_dt[i] / 1e6);
+						} else {
+							/* fall back to angular rate */
+							gyro[j] = sensors.gyro_rad_s[i * 3 + j];
+						}
+					}
+
+					_voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]);
+				}
+				_voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3],
+					sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]);
+				_voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3],
+					sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]);
+			}
+
+			int best_gyro, best_accel, best_mag;
+
+			// Get best measurement values
+			hrt_abstime curr_time = hrt_absolute_time();
+			_gyro.set(_voter_gyro.get_best(curr_time, &best_gyro));
+			_accel.set(_voter_accel.get_best(curr_time, &best_accel));
+			_mag.set(_voter_mag.get_best(curr_time, &best_mag));
+
+			if (_accel.length() < 0.01f || _mag.length() < 0.01f) {
+				warnx("WARNING: degenerate accel / mag!");
+				continue;
+			}
 
 			_data_good = true;
+
+			if (!_failsafe && (_voter_gyro.failover_count() > 0 ||
+				_voter_accel.failover_count() > 0 ||
+				_voter_mag.failover_count() > 0)) {
+
+				_failsafe = true;
+				mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
+			}
+
+			if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
+				_voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
+				_voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) {
+
+				if (_vibration_warning_timestamp == 0) {
+					_vibration_warning_timestamp = curr_time;
+				} else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000) {
+					_vibration_warning = true;
+					mavlink_and_console_log_critical(_mavlink_fd, "HIGH VIBRATION! g: %d a: %d m: %d",
+						(int)(100 * _voter_gyro.get_vibration_factor(curr_time)),
+						(int)(100 * _voter_accel.get_vibration_factor(curr_time)),
+						(int)(100 * _voter_mag.get_vibration_factor(curr_time)));
+				}
+			} else {
+				_vibration_warning_timestamp = 0;
+			}
 		}
 
 		bool gpos_updated;
@@ -311,8 +417,11 @@ void AttitudeEstimatorQ::task_main() {
 		att.pitch = euler(1);
 		att.yaw = euler(2);
 
-		att.rollspeed = _rates(0);
-		att.pitchspeed = _rates(1);
+		/* the complimentary filter should reflect the true system
+		 * state, but we need smoother outputs for the control system
+		 */
+		att.rollspeed = _lp_roll_rate.apply(_rates(0));
+		att.pitchspeed = _lp_pitch_rate.apply(_rates(1));
 		att.yawspeed = _rates(2);
 
 		for (int i = 0; i < 3; i++) {
@@ -328,6 +437,10 @@ void AttitudeEstimatorQ::task_main() {
 		memcpy(&att.R[0], R.data, sizeof(att.R));
 		att.R_valid = true;
 
+		att.rate_vibration = _voter_gyro.get_vibration_factor(hrt_absolute_time());
+		att.accel_vibration = _voter_accel.get_vibration_factor(hrt_absolute_time());
+		att.mag_vibration = _voter_mag.get_vibration_factor(hrt_absolute_time());
+
 		if (_att_pub == nullptr) {
 			_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
 		} else {
@@ -358,6 +471,7 @@ void AttitudeEstimatorQ::update_parameters(bool force) {
 		param_get(_params_handles.acc_comp, &acc_comp_int);
 		_acc_comp = acc_comp_int != 0;
 		param_get(_params_handles.bias_max, &_bias_max);
+		param_get(_params_handles.vibe_thresh, &_vibration_warning_threshold);
 	}
 }
 
@@ -501,6 +615,7 @@ int attitude_estimator_q_main(int argc, char *argv[]) {
 
 	if (!strcmp(argv[1], "status")) {
 		if (attitude_estimator_q::instance) {
+			attitude_estimator_q::instance->print();
 			warnx("running");
 			return 0;