Merge remote-tracking branch 'upstream/master' into update

.gitmodules

@@ -13,3 +13,6 @@
 [submodule "modules/gbenchmark"]
 	path = modules/gbenchmark
 	url = git://github.com/google/benchmark.git
+[submodule "modules/mavlink"]
+	path = modules/mavlink
+	url = git://github.com/diydrones/mavlink

APMrover2/wscript

@@ -24,6 +24,7 @@ def build(bld):
             'AP_ServoRelayEvents',
             'PID',
         ],
+        use='mavlink',
     )
 
     ardupilotwaf.program(

AntennaTracker/wscript

@@ -12,6 +12,7 @@ def build(bld):
         libraries=ardupilotwaf.COMMON_VEHICLE_DEPENDENT_LIBRARIES + [
             'PID',
         ],
+        use='mavlink',
     )
 
     ardupilotwaf.program(

ArduCopter/wscript

@@ -33,6 +33,7 @@ def build(bld):
             'AP_Relay',
             'AP_ServoRelayEvents',
         ],
+        use='mavlink',
     )
 
     ardupilotwaf.program(

ArduPlane/wscript

@@ -34,6 +34,7 @@ def build(bld):
             'AP_Motors',
             'AC_PID'
         ],
+        use='mavlink',
     )
 
     ardupilotwaf.program(

Makefile.waf

@@ -7,13 +7,12 @@ EXPLICIT_COMMANDS = check clean list_boards
 
 VEHICLES = copter plane rover
 
-copter_WAF_TARGET = ArduCopter
-plane_WAF_TARGET = ArduPlane
-rover_WAF_TARGET = APMrover2
-
 all: $(WAF_BINARY)
 	@$(WAF) build
 
+check-all: $(WAF_BINARY)
+	@$(WAF) check --alltests
+
 $(WAF_BINARY):
 	@git submodule init && git submodule update
 
@@ -28,7 +27,7 @@ $(EXPLICIT_COMMANDS): $(WAF_BINARY)
 
 $(VEHICLES): $(WAF_BINARY)
 	@echo Build for vehicle $@
-	@$(WAF) build --target $($@_WAF_TARGET)
+	@$(WAF) $@
 
 .DEFAULT: %-configure
 	@$(WAF) configure --board $@ build
@@ -65,6 +64,13 @@ help:
 	@echo "board/platform, you'll need to configure the build before (e.g"
 	@echo "make linux-configure)"
 	@echo ""
+	@echo "Check"
+	@echo "-----"
+	@echo "Check targets are used for running tests. There are two targets available:"
+	@echo "    check:     for running tests that are still failing or that are new or"
+	@echo "               have been modified"
+	@echo "    check-all: to run all tests"
+	@echo ""
 	@echo "Waf commands"
 	@echo "------------"
 	@echo "Waf commands can be explicitly called with targets prefixed by 'waf-'. Example:"

README_waf.txt

@@ -26,11 +26,15 @@ It's possible to build for just a vehicle or an example by specifying it as the
 target:
 
     # From the top directory
-    waf --target ArduCopter
+    waf --targets bin/ArduCopter
 
     # List all the targets available
     waf list
 
+There are also shortcuts for vehicle builds, for example:
+    # Shortcut for waf --targets bin/ArduCopter
+    waf copter
+
 By default all the files produced by the build will be inside the build/
 subdirectory. The binaries will also be there, with the name identifying
 the target board.

Tools/CPUInfo/wscript

@@ -7,4 +7,5 @@ def build(bld):
     ardupilotwaf.program(
         bld,
         use='ap',
+        blddestdir='tools',
     )

Tools/Hello/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

Tools/Replay/wscript

@@ -7,4 +7,5 @@ def build(bld):
     ardupilotwaf.program(
         bld,
         use='ap',
+        blddestdir='tools',
     )

Tools/ardupilotwaf/ardupilotwaf.py

@@ -2,7 +2,7 @@
 # encoding: utf-8
 
 from __future__ import print_function
-from waflib import Logs, Utils
+from waflib import Logs, Options, Utils
 
 SOURCE_EXTS = [
     '*.S',
@@ -74,7 +74,10 @@ def get_all_libraries(bld):
     libraries.extend(['AP_HAL', 'AP_HAL_Empty'])
     return libraries
 
-def program(bld, **kw):
+def program(bld, blddestdir='bin',
+            use_legacy_defines=True,
+            program_name=None,
+            **kw):
     if 'target' in kw:
         bld.fatal('Do not pass target for program')
     if 'defines' not in kw:
@@ -82,18 +85,26 @@ def program(bld, **kw):
     if 'source' not in kw:
         kw['source'] = bld.path.ant_glob(SOURCE_EXTS)
 
-    name = bld.path.name
+    if not program_name:
-    kw['defines'].extend(_get_legacy_defines(name))
+        program_name = bld.path.name
+
+    if use_legacy_defines:
+        kw['defines'].extend(_get_legacy_defines(program_name))
 
     kw['features'] = common_features(bld) + kw.get('features', [])
 
-    target = bld.bldnode.make_node(name + '.' + bld.env.BOARD)
+    target = blddestdir + '/' + program_name
+
     bld.program(
         target=target,
-        name=name,
+        name=target,
         **kw
     )
 
+def example(bld, **kw):
+    kw['blddestdir'] = 'examples'
+    program(bld, **kw)
+
 # NOTE: Code in libraries/ is compiled multiple times. So ensure each
 # compilation is independent by providing different index for each.
 # The need for this should disappear when libraries change to be
@@ -129,15 +140,12 @@ def vehicle_stlib(bld, **kw):
         lib_sources = lib_node.ant_glob(SOURCE_EXTS + UTILITY_SOURCE_EXTS)
         sources.extend(lib_sources)
 
-    name = kw['name']
+    kw['source'] = sources
-    vehicle = kw['vehicle']
+    kw['target'] = kw['name']
+    kw['defines'] = _get_legacy_defines(kw['vehicle'])
+    kw['idx'] = _get_next_idx()
 
-    bld.stlib(
+    bld.stlib(**kw)
-        source=sources,
-        target=name,
-        defines=_get_legacy_defines(vehicle),
-        idx=_get_next_idx(),
-    )
 
 def find_tests(bld, use=[]):
     if not bld.env.HAS_GTEST:
@@ -153,13 +161,15 @@ def find_tests(bld, use=[]):
     includes = [bld.srcnode.abspath() + '/tests/']
 
     for f in bld.path.ant_glob(incl='*.cpp'):
-        target = f.change_ext('.' + bld.env.BOARD)
+        program(
-        bld.program(
+            bld,
             features=features,
-            target=target,
             includes=includes,
             source=[f],
             use=use,
+            program_name=f.change_ext('').name,
+            blddestdir='tests',
+            use_legacy_defines=False,
         )
 
 def find_benchmarks(bld, use=[]):
@@ -169,13 +179,15 @@ def find_benchmarks(bld, use=[]):
     includes = [bld.srcnode.abspath() + '/benchmarks/']
 
     for f in bld.path.ant_glob(incl='*.cpp'):
-        target = f.change_ext('.' + bld.env.BOARD)
+        program(
-        bld.program(
+            bld,
             features=common_features(bld) + ['gbenchmark'],
-            target=target,
             includes=includes,
             source=[f],
             use=use,
+            program_name=f.change_ext('').name,
+            blddestdir='benchmarks',
+            use_legacy_defines=False,
         )
 
 def test_summary(bld):
@@ -219,3 +231,15 @@ def test_summary(bld):
 
     for filename in fails:
         Logs.error('    %s' % filename)
+
+def build_shortcut(targets=None):
+    def build_fn(bld):
+        if targets:
+            if Options.options.targets:
+                Options.options.targets += ',' + targets
+            else:
+                Options.options.targets = targets
+
+        Options.commands = ['build'] + Options.commands
+
+    return build_fn

Tools/ardupilotwaf/boards.py

@@ -91,9 +91,8 @@ def sitl(env):
 
     env.LIB += [
         'm',
-        'pthread',
     ]
-
+    env.LINKFLAGS += ['-pthread',]
     env.AP_LIBRARIES += [
         'AP_HAL_SITL',
         'SITL',
@@ -114,10 +113,9 @@ def linux(env):
 
     env.LIB += [
         'm',
-        'pthread',
         'rt',
     ]
-
+    env.LINKFLAGS += ['-pthread',]
     env.AP_LIBRARIES = [
         'AP_HAL_Linux',
     ]

Tools/ardupilotwaf/gbenchmark.py

@@ -91,7 +91,7 @@ def configure(cfg):
 
     env.INCLUDES_GBENCHMARK = [prefix_node.make_node('include').abspath()]
     env.LIBPATH_GBENCHMARK = [prefix_node.make_node('lib').abspath()]
-    env.LIB_GBENCHMARK = ['benchmark','pthread']
+    env.LIB_GBENCHMARK = ['benchmark']
 
     env.HAS_GBENCHMARK = True
 

Tools/ardupilotwaf/mavgen.py

@@ -0,0 +1,63 @@
+#!/usr/bin/env python
+# encoding: utf-8
+# (c) Siddharth Bharat Purohit, 3DRobotics Inc.
+
+"""
+The **mavgen.py** program is a code generator which creates mavlink header files.
+"""
+
+from waflib import Task, Utils, Node
+from waflib.TaskGen import feature, before_method, extension
+import os
+
+class mavgen(Task.Task):
+    """generate mavlink header files"""
+    color   = 'GREEN'
+    run_str = '${PYTHON} ${MAVGEN} --lang=C --wire-protocol=1.0 --output ${OUTPUT_DIR} ${SRC}'
+    before  = 'cxx c'
+
+    def post_run(self):
+        super(mavgen, self).post_run()
+        for header in self.generator.output_dir.ant_glob("*.h **/*.h", remove=False):
+            header.sig = header.cache_sig = self.cache_sig
+
+def options(opt):
+    opt.load('python')
+
+@feature('mavgen')
+@before_method('process_source')
+def process_mavgen(self):
+    if not hasattr(self, 'output_dir'):
+        self.bld.fatal('mavgen: missing option output_dir')
+
+    inputs = self.to_nodes(self.source)
+    outputs = []
+
+    self.source = []
+
+    if not isinstance(self.output_dir, Node.Node):
+        self.output_dir = self.bld.bldnode.find_or_declare(self.output_dir)
+
+    task = self.create_task('mavgen', inputs, outputs)
+    task.env['OUTPUT_DIR'] = self.output_dir.abspath()
+
+    task.env.env = dict(os.environ)
+    task.env.env['PYTHONPATH'] = task.env.MAVLINK_DIR
+
+def configure(cfg):
+    """
+    setup environment for mavlink header generator
+    """
+    cfg.load('python')
+    cfg.check_python_version(minver=(2,7,0))
+
+    env = cfg.env
+
+    cfg.start_msg('Checking for mavgen')
+    if not cfg.srcnode.find_resource('modules/mavlink/pymavlink/tools/mavgen.py'):
+        cfg.fatal('not found, please run: git submodule init && git submodule update')
+        return
+    cfg.end_msg('yes')
+
+    env.MAVLINK_DIR = cfg.srcnode.find_dir('modules/mavlink/').abspath()
+    env.MAVGEN = env.MAVLINK_DIR  + '/pymavlink/tools/mavgen.py'

Tools/scripts/build_all.sh

@@ -8,6 +8,9 @@
 set -e
 set -x
 
+export BUILDROOT="/tmp/all.build"
+rm -rf $BUILDROOT
+
 echo "Testing ArduPlane build"
 pushd ArduPlane
 for b in sitl linux; do

Tools/scripts/build_binaries.sh

@@ -170,7 +170,7 @@ build_arduplane() {
             continue
         }
         extension=$(board_extension $b)
-	copyit $TMPDIR/ArduPlane.build/ArduPlane.$extension $ddir $tag
+	copyit $BUILDROOT/ArduPlane.$extension $ddir $tag
 	touch $binaries/Plane/$tag
     done
     echo "Building ArduPlane PX4 binaries"
@@ -226,7 +226,7 @@ build_arducopter() {
                 continue
             }
             extension=$(board_extension $b)
-	    copyit $TMPDIR/ArduCopter.build/ArduCopter.$extension $ddir $tag
+	    copyit $BUILDROOT/ArduCopter.$extension $ddir $tag
 	    touch $binaries/Copter/$tag
         done
     done
@@ -270,7 +270,7 @@ build_rover() {
             continue
         }
         extension=$(board_extension $b)
-	copyit $TMPDIR/APMrover2.build/APMrover2.$extension $ddir $tag
+	copyit $BUILDROOT/APMrover2.$extension $ddir $tag
 	touch $binaries/Rover/$tag
     done
     echo "Building APMrover2 PX4 binaries"
@@ -312,7 +312,7 @@ build_antennatracker() {
             continue
         }
         extension=$(board_extension $b)
-	copyit $TMPDIR/AntennaTracker.build/AntennaTracker.$extension $ddir $tag
+	copyit $BUILDROOT/AntennaTracker.$extension $ddir $tag
 	touch $binaries/AntennaTracker/$tag
     done
     echo "Building AntennaTracker PX4 binaries"
@@ -342,6 +342,9 @@ build_antennatracker() {
     git submodule update
 }
 
+export BUILDROOT="$TMPDIR/binaries.build"
+rm -rf $BUILDROOT
+
 # make sure PX4 is rebuilt from scratch
 for d in ArduPlane ArduCopter APMrover2 AntennaTracker; do
          pushd $d

Tools/scripts/build_examples.sh

@@ -6,7 +6,7 @@
 set -e
 set -x
 
-targets="clean navio"
+targets="navio"
 
 [ $# -gt 0 ] && {
     targets="$*"
@@ -16,6 +16,9 @@ export PATH=/usr/lib/ccache:$PATH
 
 TESTS=$(find libraries -name 'Makefile' | grep -v FLYMAPLE | xargs -i dirname '{}')
 
+export BUILDROOT="/tmp/examples.build"
+rm -rf $BUILDROOT
+
 for b in $TESTS; do
     echo "TESTING $b"
     pushd $b

libraries/AC_PID/examples/AC_PID_test/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_ADC/examples/AP_ADC_test/wscript

@@ -7,7 +7,7 @@ def build(bld):
     if bld.env.BOARD in ['sitl']:
         return
 
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_AHRS/examples/AHRS_Test/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_Airspeed/examples/Airspeed/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_Baro/examples/BARO_generic/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_BattMonitor/examples/AP_BattMonitor_test/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_Common/examples/AP_Common/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_Compass/examples/AP_Compass_test/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_Declination/examples/AP_Declination_test/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_GPS/examples/GPS_AUTO_test/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_GPS/examples/GPS_UBLOX_passthrough/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL/examples/AnalogIn/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL/examples/Printf/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL/examples/RCInput/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL/examples/RCInputToRCOutput/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL/examples/RCOutput/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL/examples/Storage/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL/examples/UART_test/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL_AVR/examples/ArduCopterLibs/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL_AVR/examples/ArduPlaneLibs/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL_AVR/examples/Blink/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL_AVR/examples/Console/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL_AVR/examples/I2CDriver_HMC5883L/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL_AVR/examples/LCDTest/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL_AVR/examples/RCInputTest/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL_AVR/examples/RCJitterTest/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL_AVR/examples/RCPassthroughTest/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL_AVR/examples/SPIDriver_MPU6000/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL_AVR/examples/Scheduler/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL_AVR/examples/Semaphore/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL_AVR/examples/Storage/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL_AVR/examples/UARTDriver/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL_AVR/examples/UtilityStringTest/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL_FLYMAPLE/examples/AP_Baro_BMP085_test/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL_FLYMAPLE/examples/AnalogIn/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL_FLYMAPLE/examples/Blink/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL_FLYMAPLE/examples/Console/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL_FLYMAPLE/examples/I2CDriver_HMC5883L/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL_FLYMAPLE/examples/RCInput/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL_FLYMAPLE/examples/RCPassthroughTest/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL_FLYMAPLE/examples/SPIDriver/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL_FLYMAPLE/examples/Scheduler/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL_FLYMAPLE/examples/Semaphore/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL_FLYMAPLE/examples/Storage/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL_FLYMAPLE/examples/UARTDriver/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL_FLYMAPLE/examples/UtilityStringTest/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL_FLYMAPLE/examples/empty_example/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h

@@ -51,6 +51,7 @@ namespace Linux {
     class VideoIn;
     class OpticalFlow_Onboard;
     class Flow_PX4;
+    class Perf_Lttng;
 }
 
 #endif // __AP_HAL_LINUX_NAMESPACE_H__

libraries/AP_HAL_Linux/OpticalFlow_Onboard.cpp

@@ -117,22 +117,42 @@ void OpticalFlow_Onboard::init(AP_HAL::OpticalFlow::Gyro_Cb get_gyro)
         AP_HAL::panic("OpticalFlow_Onboard: format not supported\n");
     }
 
-    if (_videoin->set_crop(left, top, crop_width, crop_height)) {
+    if (_width == HAL_OPTFLOW_ONBOARD_OUTPUT_WIDTH &&
-        _crop_by_software = false;
+        _height == HAL_OPTFLOW_ONBOARD_OUTPUT_HEIGHT) {
+        _shrink_by_software = false;
     } else {
-        /* here we store the actual camera output width and height to use them
+        /* here we store the actual camera output width and height to use
-         * later on to software crop each frame. */
+         * them later on to software shrink each frame. */
-        _crop_by_software = true;
+        _shrink_by_software = true;
-        _crop_by_software_width = _width;
+        _camera_output_width = _width;
-        _crop_by_software_height = _height;
+        _camera_output_height = _height;
 
         /* we set these values here in order to the calculations be correct
-         * (such as PX4 init) even though we crop each frame later on. */
+         * (such as PX4 init) even though we shrink each frame later on. */
         _width = HAL_OPTFLOW_ONBOARD_OUTPUT_WIDTH;
         _height = HAL_OPTFLOW_ONBOARD_OUTPUT_HEIGHT;
         _bytesperline = HAL_OPTFLOW_ONBOARD_OUTPUT_WIDTH;
     }
 
+    if (_videoin->set_crop(left, top, crop_width, crop_height)) {
+        _crop_by_software = false;
+    } else {
+        _crop_by_software = true;
+
+        if (!_shrink_by_software) {
+            /* here we store the actual camera output width and height to use
+             * them later on to software crop each frame. */
+            _camera_output_width = _width;
+            _camera_output_height = _height;
+
+            /* we set these values here in order to the calculations be correct
+             * (such as PX4 init) even though we crop each frame later on. */
+            _width = HAL_OPTFLOW_ONBOARD_OUTPUT_WIDTH;
+            _height = HAL_OPTFLOW_ONBOARD_OUTPUT_HEIGHT;
+            _bytesperline = HAL_OPTFLOW_ONBOARD_OUTPUT_WIDTH;
+        }
+    }
+
     if (!_videoin->allocate_buffers(nbufs)) {
         AP_HAL::panic("OpticalFlow_Onboard: couldn't allocate video buffers");
     }
@@ -208,14 +228,16 @@ void OpticalFlow_Onboard::_run_optflow()
     Vector3f gyro_rate;
     Vector2f flow_rate;
     VideoIn::Frame video_frame;
-    uint32_t convert_buffer_size = 0, crop_buffer_size = 0;
+    uint32_t convert_buffer_size = 0, output_buffer_size = 0;
     uint32_t crop_left = 0, crop_top = 0;
-    uint8_t *convert_buffer = NULL, *crop_buffer = NULL;
+    uint32_t shrink_scale = 0, shrink_width = 0, shrink_height = 0;
+    uint32_t shrink_width_offset = 0, shrink_height_offset = 0;
+    uint8_t *convert_buffer = NULL, *output_buffer = NULL;
     uint8_t qual;
 
     if (_format == V4L2_PIX_FMT_YUYV) {
-        if (_crop_by_software) {
+        if (_shrink_by_software || _crop_by_software) {
-            convert_buffer_size = _crop_by_software_width * _crop_by_software_height;
+            convert_buffer_size = _camera_output_width * _camera_output_height;
         } else {
             convert_buffer_size = _width * _height;
         }
@@ -226,22 +248,38 @@ void OpticalFlow_Onboard::_run_optflow()
         }
     }
 
-    if (_crop_by_software) {
+    if (_shrink_by_software || _crop_by_software) {
-        crop_buffer_size = HAL_OPTFLOW_ONBOARD_OUTPUT_WIDTH *
+        output_buffer_size = HAL_OPTFLOW_ONBOARD_OUTPUT_WIDTH *
             HAL_OPTFLOW_ONBOARD_OUTPUT_HEIGHT;
 
-        crop_buffer = (uint8_t *)malloc(crop_buffer_size);
+        output_buffer = (uint8_t *)malloc(output_buffer_size);
-        if (!crop_buffer) {
+        if (!output_buffer) {
             if (convert_buffer) {
                 free(convert_buffer);
             }
 
             AP_HAL::panic("OpticalFlow_Onboard: couldn't allocate crop buffer\n");
         }
+    }
 
-        crop_left = _crop_by_software_width / 2 -
+    if (_shrink_by_software) {
+        if (_camera_output_width > _camera_output_height) {
+            shrink_scale = (uint32_t) _camera_output_height /
+                HAL_OPTFLOW_ONBOARD_OUTPUT_HEIGHT;
+        } else {
+            shrink_scale = (uint32_t) _camera_output_width /
+                HAL_OPTFLOW_ONBOARD_OUTPUT_WIDTH;
+        }
+
+        shrink_width = HAL_OPTFLOW_ONBOARD_OUTPUT_WIDTH * shrink_scale;
+        shrink_height = HAL_OPTFLOW_ONBOARD_OUTPUT_HEIGHT * shrink_scale;
+
+        shrink_width_offset = (_camera_output_width - shrink_width) / 2;
+        shrink_height_offset = (_camera_output_height - shrink_height) / 2;
+    } else if (_crop_by_software) {
+        crop_left = _camera_output_width / 2 -
            HAL_OPTFLOW_ONBOARD_OUTPUT_WIDTH / 2;
-        crop_top = _crop_by_software_height / 2 -
+        crop_top = _camera_output_height / 2 -
            HAL_OPTFLOW_ONBOARD_OUTPUT_HEIGHT / 2;
     }
 
@@ -252,8 +290,8 @@ void OpticalFlow_Onboard::_run_optflow()
                free(convert_buffer);
             }
 
-            if (crop_buffer) {
+            if (output_buffer) {
-               free(crop_buffer);
+               free(output_buffer);
             }
 
             AP_HAL::panic("OpticalFlow_Onboard: couldn't get frame\n");
@@ -267,14 +305,24 @@ void OpticalFlow_Onboard::_run_optflow()
             memcpy(video_frame.data, convert_buffer, convert_buffer_size);
         }
 
-        if (_crop_by_software) {
+        if (_shrink_by_software) {
-            VideoIn::crop_8bpp((uint8_t *)video_frame.data, crop_buffer,
+            /* shrink_8bpp() will shrink a selected area using the offsets,
-                               _crop_by_software_width,
+             * therefore, we don't need the crop. */
+            VideoIn::shrink_8bpp((uint8_t *)video_frame.data, output_buffer,
+                                 _camera_output_width, _camera_output_height,
+                                 shrink_width_offset, shrink_width,
+                                 shrink_height_offset, shrink_height,
+                                 shrink_scale, shrink_scale);
+            memset(video_frame.data, 0, _camera_output_width * _camera_output_height);
+            memcpy(video_frame.data, output_buffer, output_buffer_size);
+        } else if (_crop_by_software) {
+            VideoIn::crop_8bpp((uint8_t *)video_frame.data, output_buffer,
+                               _camera_output_width,
                                crop_left, HAL_OPTFLOW_ONBOARD_OUTPUT_WIDTH,
                                crop_top, HAL_OPTFLOW_ONBOARD_OUTPUT_HEIGHT);
 
-            memset(video_frame.data, 0, _crop_by_software_width * _crop_by_software_height);
+            memset(video_frame.data, 0, _camera_output_width * _camera_output_height);
-            memcpy(video_frame.data, crop_buffer, crop_buffer_size);
+            memcpy(video_frame.data, output_buffer, output_buffer_size);
         }
 
         /* if it is at least the second frame we receive
@@ -326,10 +374,12 @@ void OpticalFlow_Onboard::_run_optflow()
                                   HAL_FLOW_PX4_FOCAL_LENGTH_MILLIPX;
         _integration_timespan += video_frame.timestamp -
                                  _last_video_frame.timestamp;
-        _gyro_x_integral       += gyro_rate.x * (video_frame.timestamp -
+        _gyro_x_integral       += (gyro_rate.x + _last_gyro_rate.x) / 2.0f *
-                                 _last_video_frame.timestamp);
+                                  (video_frame.timestamp -
-        _gyro_y_integral       += gyro_rate.y * (video_frame.timestamp -
+                                  _last_video_frame.timestamp);
-                                 _last_video_frame.timestamp);
+        _gyro_y_integral       += (gyro_rate.y + _last_gyro_rate.y) / 2.0f *
+                                  (video_frame.timestamp -
+                                  _last_video_frame.timestamp);
         _surface_quality = qual;
         _data_available = true;
         pthread_mutex_unlock(&_mutex);
@@ -337,14 +387,15 @@ void OpticalFlow_Onboard::_run_optflow()
         /* give the last frame back to the video input driver */
         _videoin->put_frame(_last_video_frame);
         _last_video_frame = video_frame;
+        _last_gyro_rate = gyro_rate;
     }
 
     if (convert_buffer) {
         free(convert_buffer);
     }
 
-    if (crop_buffer) {
+    if (output_buffer) {
-        free(crop_buffer);
+        free(output_buffer);
     }
 }
 #endif

libraries/AP_HAL_Linux/OpticalFlow_Onboard.h

@@ -42,8 +42,9 @@ private:
     bool _initialized;
     bool _data_available;
     bool _crop_by_software;
-    uint32_t _crop_by_software_width;
+    bool _shrink_by_software;
-    uint32_t _crop_by_software_height;
+    uint32_t _camera_output_width;
+    uint32_t _camera_output_height;
     uint32_t _width;
     uint32_t _height;
     uint32_t _format;
@@ -56,4 +57,5 @@ private:
     uint32_t _integration_timespan;
     uint8_t _surface_quality;
     AP_HAL::OpticalFlow::Gyro_Cb _get_gyro;
+    Vector3f _last_gyro_rate;
 };

libraries/AP_HAL_Linux/Perf.cpp

@@ -18,7 +18,7 @@
 
 #include <AP_HAL/AP_HAL.h>
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
+#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX && !defined(PERF_LTTNG)
 
 #include <limits.h>
 #include <time.h>

libraries/AP_HAL_Linux/Perf_Lttng.cpp

@@ -0,0 +1,87 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <AP_HAL/AP_HAL.h>
+
+#pragma GCC diagnostic ignored "-Wcast-align"
+#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX && defined(PERF_LTTNG)
+#define TRACEPOINT_CREATE_PROBES
+#define TRACEPOINT_DEFINE
+#include "Perf_Lttng_TracePoints.h"
+
+#include <string.h>
+#include "Perf_Lttng.h"
+#include "AP_HAL_Linux.h"
+#include "Util.h"
+
+using namespace Linux;
+
+Perf_Lttng::Perf_Lttng(AP_HAL::Util::perf_counter_type type, const char *name)
+    : _type(type)
+{
+    strncpy(_name, name, MAX_TRACEPOINT_NAME_LEN);
+}
+
+void Perf_Lttng::begin()
+{
+    if (_type != AP_HAL::Util::PC_ELAPSED) {
+        return;
+    }
+    tracepoint(ardupilot, begin, _name);
+}
+
+void Perf_Lttng::end()
+{
+    if (_type != AP_HAL::Util::PC_ELAPSED) {
+        return;
+    }
+    tracepoint(ardupilot, end, _name);
+}
+
+void Perf_Lttng::count()
+{
+    if (_type != AP_HAL::Util::PC_COUNT) {
+        return;
+    }
+    tracepoint(ardupilot, count, _name, ++_count);
+}
+
+Util::perf_counter_t Util::perf_alloc(perf_counter_type type, const char *name)
+{
+    return new Linux::Perf_Lttng(type, name);
+}
+
+void Util::perf_begin(perf_counter_t perf)
+{
+    Linux::Perf_Lttng *perf_lttng = (Linux::Perf_Lttng *)perf;
+
+    perf_lttng->begin();
+}
+
+void Util::perf_end(perf_counter_t perf)
+{
+    Linux::Perf_Lttng *perf_lttng = (Linux::Perf_Lttng *)perf;
+
+    perf_lttng->end();
+}
+
+void Util::perf_count(perf_counter_t perf)
+{
+    Linux::Perf_Lttng *perf_lttng = (Linux::Perf_Lttng *)perf;
+
+    perf_lttng->count();
+}
+
+#endif

libraries/AP_HAL_Linux/Perf_Lttng.h

@@ -0,0 +1,32 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+#include "AP_HAL_Linux.h"
+#include <AP_HAL/Util.h>
+
+#define MAX_TRACEPOINT_NAME_LEN 128
+
+class Linux::Perf_Lttng {
+public:
+    Perf_Lttng(enum AP_HAL::Util::perf_counter_type type, const char *name);
+    void begin();
+    void end();
+    void count();
+private:
+    char _name[MAX_TRACEPOINT_NAME_LEN];
+    uint64_t _count;
+    enum AP_HAL::Util::perf_counter_type _type;
+};

libraries/AP_HAL_Linux/Perf_Lttng_TracePoints.h

@@ -0,0 +1,65 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#undef TRACEPOINT_PROVIDER
+#define TRACEPOINT_PROVIDER ardupilot
+
+#undef TRACEPOINT_INCLUDE
+#define TRACEPOINT_INCLUDE <AP_HAL_Linux/Perf_Lttng_TracePoints.h>
+
+#if !defined(_PERF_LTTNG_TRACEPOINT_H) || defined(TRACEPOINT_HEADER_MULTI_READ)
+#define _PERF_LTTNG_TRACEPOINT_H
+
+#include <lttng/tracepoint.h>
+
+TRACEPOINT_EVENT(
+    ardupilot,
+    begin,
+    TP_ARGS(
+        char*, name_arg
+    ),
+    TP_FIELDS(
+        ctf_string(name_field, name_arg)
+    )
+)
+
+TRACEPOINT_EVENT(
+    ardupilot,
+    end,
+    TP_ARGS(
+        char*, name_arg
+    ),
+    TP_FIELDS(
+        ctf_string(name_field, name_arg)
+    )
+)
+
+TRACEPOINT_EVENT(
+    ardupilot,
+    count,
+    TP_ARGS(
+        char*, name_arg,
+        int, count_arg
+    ),
+    TP_FIELDS(
+        ctf_string(name_field, name_arg)
+        ctf_integer(int, count_field, count_arg)
+    )
+)
+
+#endif /* _HELLO_TP_H */
+
+#include <lttng/tracepoint-event.h>
+

libraries/AP_HAL_Linux/RCInput.cpp

@@ -42,6 +42,11 @@ uint8_t RCInput::num_channels()
     return _num_channels;
 }
 
+void RCInput::set_num_channels(uint8_t num)
+{
+    _num_channels = num;
+}
+
 uint16_t RCInput::read(uint8_t ch) 
 {
     new_rc_input = false;
@@ -312,10 +317,18 @@ reset:
     memset(&dsm_state, 0, sizeof(dsm_state));        
 }
 
+void RCInput::_process_pwm_pulse(uint16_t channel, uint16_t width_s0, uint16_t width_s1)
+{
+    if (channel < _num_channels) {
+        _pwm_values[channel] = width_s1; // range: 700 ~ 2300
+        new_rc_input = true;
+    }
+}
+
 /*
   process a RC input pulse of the given width
  */
-void RCInput::_process_rc_pulse(uint16_t width_s0, uint16_t width_s1)
+void RCInput::_process_rc_pulse(uint16_t width_s0, uint16_t width_s1, uint16_t channel)
 {
 #if 0
     // useful for debugging
@@ -327,14 +340,23 @@ void RCInput::_process_rc_pulse(uint16_t width_s0, uint16_t width_s1)
         fprintf(rclog, "%u %u\n", (unsigned)width_s0, (unsigned)width_s1);
     }
 #endif
-    // treat as PPM-sum
-    _process_ppmsum_pulse(width_s0 + width_s1);
 
-    // treat as SBUS
+    if (channel == LINUX_RC_INPUT_CHANNEL_INVALID) {
-    _process_sbus_pulse(width_s0, width_s1);
 
-    // treat as DSM
+      // treat as PPM-sum
-    _process_dsm_pulse(width_s0, width_s1);
+      _process_ppmsum_pulse(width_s0 + width_s1);
+
+      // treat as SBUS
+      _process_sbus_pulse(width_s0, width_s1);
+
+      // treat as DSM
+      _process_dsm_pulse(width_s0, width_s1);
+
+    } else {
+
+      // treat as PWM
+      _process_pwm_pulse(channel, width_s0, width_s1);
+    }
 }
 
 /*

libraries/AP_HAL_Linux/RCInput.h

@@ -5,6 +5,7 @@
 #include "AP_HAL_Linux.h"
 
 #define LINUX_RC_INPUT_NUM_CHANNELS 16
+#define LINUX_RC_INPUT_CHANNEL_INVALID (999)
 
 class Linux::RCInput : public AP_HAL::RCInput {
 public:
@@ -17,6 +18,8 @@ public:
     virtual void init();
     bool new_input();
     uint8_t num_channels();
+    void set_num_channels(uint8_t num);
+
     uint16_t read(uint8_t ch);
     uint8_t read(uint16_t* periods, uint8_t len);
 
@@ -33,7 +36,8 @@ public:
     
     
  protected:
-    void _process_rc_pulse(uint16_t width_s0, uint16_t width_s1);
+    void _process_rc_pulse(uint16_t width_s0, uint16_t width_s1,
+                           uint16_t channel = LINUX_RC_INPUT_CHANNEL_INVALID);
     void _update_periods(uint16_t *periods, uint8_t len);
 
  private:
@@ -45,6 +49,7 @@ public:
     void _process_ppmsum_pulse(uint16_t width);
     void _process_sbus_pulse(uint16_t width_s0, uint16_t width_s1);
     void _process_dsm_pulse(uint16_t width_s0, uint16_t width_s1);
+    void _process_pwm_pulse(uint16_t channel, uint16_t width_s0, uint16_t width_s1);
 
     /* override state */
     uint16_t _override[LINUX_RC_INPUT_NUM_CHANNELS];

libraries/AP_HAL_Linux/RCInput_RPI.cpp

@@ -30,14 +30,30 @@
 #define RCIN_RPI_BUFFER_LENGTH   8
 #define RCIN_RPI_SAMPLE_FREQ     500
 #define RCIN_RPI_DMA_CHANNEL     0
-#define RCIN_RPI_MAX_COUNTER     1300
-#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH
-#define PPM_INPUT_RPI RPI_GPIO_5
-#else
-#define PPM_INPUT_RPI RPI_GPIO_4
-#endif
 #define RCIN_RPI_MAX_SIZE_LINE   50
 
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH
+// Same as the circle_buffer size
+// See comments in "init_ctrl_data()" to understand values "2"
+#define RCIN_RPI_MAX_COUNTER     (RCIN_RPI_BUFFER_LENGTH * PAGE_SIZE * 2)
+#define RCIN_RPI_SIG_HIGH        0
+#define RCIN_RPI_SIG_LOW         1
+// Each gpio stands for a rcinput channel,
+// the first one in RcChnGpioTbl is channel 1 in receiver
+static uint16_t RcChnGpioTbl[RCIN_RPI_CHN_NUM] = {
+    RPI_GPIO_5,  RPI_GPIO_6,  RPI_GPIO_12,
+    RPI_GPIO_13, RPI_GPIO_19, RPI_GPIO_20,
+    RPI_GPIO_21, RPI_GPIO_26
+};
+#else
+#define RCIN_RPI_MAX_COUNTER     10432
+#define RCIN_RPI_SIG_HIGH        1
+#define RCIN_RPI_SIG_LOW         0
+static uint16_t RcChnGpioTbl[RCIN_RPI_CHN_NUM] = {
+    RPI_GPIO_4
+};
+#endif
+
 //Memory Addresses
 #define RCIN_RPI_RPI1_DMA_BASE 0x20007000
 #define RCIN_RPI_RPI1_CLK_BASE 0x20101000
@@ -54,7 +70,7 @@
 #define RCIN_RPI_TIMER_BASE      0x7e003004
 
 #define RCIN_RPI_DMA_SRC_INC     (1<<8)
-#define RCIN_RPI_DMA_DEST_INC    (1<<4) 
+#define RCIN_RPI_DMA_DEST_INC    (1<<4)
 #define RCIN_RPI_DMA_NO_WIDE_BURSTS  (1<<26)
 #define RCIN_RPI_DMA_WAIT_RESP   (1<<3)
 #define RCIN_RPI_DMA_D_DREQ      (1<<6)
@@ -108,7 +124,7 @@ Memory_table::Memory_table(uint32_t page_count, int version)
     _virt_pages = (void**)malloc(page_count * sizeof(void*));
     _phys_pages = (void**)malloc(page_count * sizeof(void*));
     _page_count = page_count;
- 
+
     if ((fdMem = open("/dev/mem", O_RDWR | O_SYNC)) < 0) {
         fprintf(stderr,"Failed to open /dev/mem\n");
         exit(-1);
@@ -126,7 +142,7 @@ Memory_table::Memory_table(uint32_t page_count, int version)
     //Get list of available cache coherent physical addresses
     for (i = 0; i < _page_count; i++) {
         _virt_pages[i]  =  mmap(0, PAGE_SIZE, PROT_READ|PROT_WRITE, MAP_SHARED|MAP_ANONYMOUS|MAP_NORESERVE|MAP_LOCKED,-1,0);
-        ::read(file, &pageInfo, 8); 
+        ::read(file, &pageInfo, 8);
         _phys_pages[i] = (void*)((uintptr_t)(pageInfo*PAGE_SIZE) | bus);
     }
 
@@ -146,20 +162,19 @@ Memory_table::~Memory_table()
     free(_phys_pages);
 }
 
-// This function returns physical address with help of pointer, which is offset from the beginning of the buffer.
+/* Return physical address with help of pointer, which is offset from the
+ * beginning of the buffer. */
 void* Memory_table::get_page(void** const pages, uint32_t addr) const
 {
     if (addr >= PAGE_SIZE * _page_count) {
-        return NULL;   
+        return NULL;
     }
     return (uint8_t*)pages[(uint32_t) addr / 4096] + addr % 4096;
 }
 
-//Get virtual address from the corresponding physical address from memory_table.
+// Get virtual address from the corresponding physical address from memory_table.
-void* Memory_table::get_virt_addr(const uint32_t phys_addr) const
+void *Memory_table::get_virt_addr(const uint32_t phys_addr) const
 {
-    // FIXME: Can't the address be calculated directly?
-    // FIXME: if the address room  in _phys_pages is not fragmented one may avoid a complete loop ..
     uint32_t i = 0;
     for (; i < _page_count; i++) {
         if ((uintptr_t) _phys_pages[i] == (((uintptr_t) phys_addr) & 0xFFFFF000)) {
@@ -169,14 +184,13 @@ void* Memory_table::get_virt_addr(const uint32_t phys_addr) const
     return NULL;
 }
 
-// FIXME: in-congruent function style see above
+// Return offset from the beginning of the buffer using virtual address and memory_table.
-// This function returns offset from the beginning of the buffer using virtual address and memory_table.
 uint32_t Memory_table::get_offset(void ** const pages, const uint32_t addr) const
 {
     uint32_t i = 0;
     for (; i < _page_count; i++) {
         if ((uintptr_t) pages[i] == (addr & 0xFFFFF000) ) {
-            return (i*PAGE_SIZE + (addr & 0xFFF));
+            return (i * PAGE_SIZE + (addr & 0xFFF));
         }
     }
     return -1;
@@ -187,8 +201,7 @@ uint32_t Memory_table::bytes_available(const uint32_t read_addr, const uint32_t
 {
     if (write_addr > read_addr) {
         return (write_addr - read_addr);
-    }
+    } else {
-    else {
         return _page_count * PAGE_SIZE - (read_addr - write_addr);
     }
 }
@@ -205,8 +218,7 @@ void RCInput_RPI::set_physical_addresses(int version)
         dma_base = RCIN_RPI_RPI1_DMA_BASE;
         clk_base = RCIN_RPI_RPI1_CLK_BASE;
         pcm_base = RCIN_RPI_RPI1_PCM_BASE;
-    }                                                                                                                                     
+    } else if (version == 2) {
-    else if (version == 2) {
         dma_base = RCIN_RPI_RPI2_DMA_BASE;
         clk_base = RCIN_RPI_RPI2_CLK_BASE;
         pcm_base = RCIN_RPI_RPI2_PCM_BASE;
@@ -264,69 +276,68 @@ void RCInput_RPI::init_ctrl_data()
     uint32_t cbp = 0;
     dma_cb_t* cbp_curr;
     //Set fifo addr (for delay)
-    phys_fifo_addr = ((pcm_base + 0x04) & 0x00FFFFFF) | 0x7e000000;  
+    phys_fifo_addr = ((pcm_base + 0x04) & 0x00FFFFFF) | 0x7e000000;
-    
+
     //Init dma control blocks.
-    /*We are transferring 1 byte of GPIO register. Every 56th iteration we are 
+    /*We are transferring 8 bytes of GPIO register. Every 7th iteration we are
-      sampling TIMER register, which length is 8 bytes. So, for every 56 samples of GPIO we need 
+      sampling TIMER register, which length is 8 bytes. So, for every 7 samples of GPIO we need
-      56 * 1 + 8 = 64 bytes of buffer. Value 56 was selected specially to have a 64-byte "block" 
+      7 * 8 + 8 = 64 bytes of buffer. Value 7 was selected specially to have a 64-byte "block"
-      TIMER - GPIO. So, we have integer count of such "blocks" at one virtual page. (4096 / 64 = 64 
+      TIMER - GPIO. So, we have integer count of such "blocks" at one virtual page. (4096 / 64 = 64
-      "blocks" per page. As minimum, we must have 2 virtual pages of buffer (to have integer count of 
+      "blocks" per page. As minimum, we must have 2 virtual pages of buffer (to have integer count of
-      vitual pages for control blocks): for every 56 iterations (64 bytes of buffer) we need 56 control blocks for GPIO
+      vitual pages for control blocks): for every 7 iterations (64 bytes of buffer) we need 7 control blocks for GPIO
-      sampling, 56 control blocks for setting frequency and 1 control block for sampling timer, so,
+      sampling, 7 control blocks for setting frequency and 1 control block for sampling timer, so,
-      we need 56 + 56 + 1 = 113 control blocks. For integer value, we need 113 pages of control blocks.
+      we need 7 + 7 + 1 = 15 control blocks. For integer value, we need 15 pages of control blocks.
-      Each control block length is 32 bytes. In 113 pages we will have (113 * 4096 / 32) = 113 * 128 control
+      Each control block length is 32 bytes. In 15 pages we will have (15 * 4096 / 32) = 15 * 128 control
-      blocks. 113 * 128 control blocks = 64 * 128 bytes of buffer = 2 pages of buffer.
+      blocks. 15 * 128 control blocks = 64 * 128 bytes of buffer = 2 pages of buffer.
-      So, for 56 * 64 * 2 iteration we init DMA for sampling GPIO
+      So, for 7 * 64 * 2 iteration we init DMA for sampling GPIO
-      and timer to (64 * 64 * 2) = 8192 bytes = 2 pages of buffer.
+      and timer to ((7 * 8 + 8) * 64 * 2) = 8192 bytes = 2 pages of buffer.
     */
     //    fprintf(stderr, "ERROR SEARCH1\n");
 
-  uint32_t i = 0;
+    uint32_t i = 0;
-  for (i = 0; i < 56 * 128 * RCIN_RPI_BUFFER_LENGTH; i++) // 8 * 56 * 128 == 57344
+    for (i = 0; i < 7 * 128 * RCIN_RPI_BUFFER_LENGTH; i++) {
-	{
+        //Transfer timer every 7th sample
-      //Transfer timer every 56th sample
+        if (i % 7 == 0) {
-      if(i % 56 == 0) {
+            cbp_curr = (dma_cb_t*)con_blocks->get_page(con_blocks->_virt_pages, cbp);
-          cbp_curr = (dma_cb_t*)con_blocks->get_page(con_blocks->_virt_pages, cbp);
 
-          init_dma_cb(&cbp_curr, RCIN_RPI_DMA_NO_WIDE_BURSTS | RCIN_RPI_DMA_WAIT_RESP | RCIN_RPI_DMA_DEST_INC | RCIN_RPI_DMA_SRC_INC, RCIN_RPI_TIMER_BASE, 
+            init_dma_cb(&cbp_curr, RCIN_RPI_DMA_NO_WIDE_BURSTS | RCIN_RPI_DMA_WAIT_RESP | RCIN_RPI_DMA_DEST_INC | RCIN_RPI_DMA_SRC_INC, RCIN_RPI_TIMER_BASE,
-		      (uintptr_t) circle_buffer->get_page(circle_buffer->_phys_pages, dest), 
+                        (uintptr_t) circle_buffer->get_page(circle_buffer->_phys_pages, dest),
-		      8, 
+                        8,
-		      0, 
+                        0,
-		      (uintptr_t) con_blocks->get_page(con_blocks->_phys_pages, 
+                        (uintptr_t) con_blocks->get_page(con_blocks->_phys_pages,
-		      cbp + sizeof(dma_cb_t) ) );
+                                                         cbp + sizeof(dma_cb_t) ) );
-	  
-          dest += 8;
-          cbp += sizeof(dma_cb_t);
-      } 
 
-	    // Transfer GPIO (1 byte)
+            dest += 8;
-	    cbp_curr = (dma_cb_t*)con_blocks->get_page(con_blocks->_virt_pages, cbp);
+            cbp += sizeof(dma_cb_t);
-	    init_dma_cb(&cbp_curr, RCIN_RPI_DMA_NO_WIDE_BURSTS | RCIN_RPI_DMA_WAIT_RESP, RCIN_RPI_GPIO_LEV0_ADDR, 
+        }
-			(uintptr_t) circle_buffer->get_page(circle_buffer->_phys_pages, dest), 
-			1, 
-			0, 
-			(uintptr_t) con_blocks->get_page(con_blocks->_phys_pages, 
-			cbp + sizeof(dma_cb_t) ) );
-	    
-	    dest += 1;
-	    cbp += sizeof(dma_cb_t);	  
 
-	    // Delay (for setting sampling frequency)
+        // Transfer GPIO (8 bytes)
-	    /* DMA is waiting data request signal (DREQ) from PCM. PCM is set for 1 MhZ freqency, so,
+        cbp_curr = (dma_cb_t*)con_blocks->get_page(con_blocks->_virt_pages, cbp);
-	       each sample of GPIO is limited by writing to PCA queue.
+        init_dma_cb(&cbp_curr, RCIN_RPI_DMA_NO_WIDE_BURSTS | RCIN_RPI_DMA_WAIT_RESP, RCIN_RPI_GPIO_LEV0_ADDR,
-	    */
+                    (uintptr_t) circle_buffer->get_page(circle_buffer->_phys_pages, dest),
-	    cbp_curr = (dma_cb_t*)con_blocks->get_page(con_blocks->_virt_pages, cbp);
+                    8,
-	    init_dma_cb(&cbp_curr, RCIN_RPI_DMA_NO_WIDE_BURSTS | RCIN_RPI_DMA_WAIT_RESP | RCIN_RPI_DMA_D_DREQ | RCIN_RPI_DMA_PER_MAP(2), 
+                    0,
-			RCIN_RPI_TIMER_BASE, phys_fifo_addr, 
+                    (uintptr_t) con_blocks->get_page(con_blocks->_phys_pages,
-			4, 
+                                                     cbp + sizeof(dma_cb_t) ) );
-			0, 
+
-			(uintptr_t)con_blocks->get_page(con_blocks->_phys_pages, 
+        dest += 8;
-			cbp + sizeof(dma_cb_t) ) );
+        cbp += sizeof(dma_cb_t);
-	    
+
-	    cbp += sizeof(dma_cb_t);
+        // Delay (for setting sampling frequency)
-	}
+        /* DMA is waiting data request signal (DREQ) from PCM. PCM is set for 1 MhZ freqency, so,
-    //Make last control block point to the first (to make circle) 
+           each sample of GPIO is limited by writing to PCA queue.
+           */
+        cbp_curr = (dma_cb_t*)con_blocks->get_page(con_blocks->_virt_pages, cbp);
+        init_dma_cb(&cbp_curr, RCIN_RPI_DMA_NO_WIDE_BURSTS | RCIN_RPI_DMA_WAIT_RESP | RCIN_RPI_DMA_D_DREQ | RCIN_RPI_DMA_PER_MAP(2),
+                    RCIN_RPI_TIMER_BASE, phys_fifo_addr,
+                    4,
+                    0,
+                    (uintptr_t)con_blocks->get_page(con_blocks->_phys_pages,
+                                                    cbp + sizeof(dma_cb_t) ) );
+
+        cbp += sizeof(dma_cb_t);
+    }
+    //Make last control block point to the first (to make circle)
     cbp -= sizeof(dma_cb_t);
     ((dma_cb_t*)con_blocks->get_page(con_blocks->_virt_pages, cbp))->next = (uintptr_t) con_blocks->get_page(con_blocks->_phys_pages, 0);
 }
@@ -335,7 +346,7 @@ void RCInput_RPI::init_ctrl_data()
 /*Initialise PCM
   See BCM2835 documentation:
   http://www.raspberrypi.org/wp-content/uploads/2012/02/BCM2835-ARM-Peripherals.pdf
-*/  
+*/
 void RCInput_RPI::init_PCM()
 {
     pcm_reg[RCIN_RPI_PCM_CS_A] = 1;                                          // Disable Rx+Tx, Enable PCM block
@@ -363,7 +374,7 @@ void RCInput_RPI::init_PCM()
 /*Initialise DMA
   See BCM2835 documentation:
   http://www.raspberrypi.org/wp-content/uploads/2012/02/BCM2835-ARM-Peripherals.pdf
-*/  
+*/
 void RCInput_RPI::init_DMA()
 {
     dma_reg[RCIN_RPI_DMA_CS | RCIN_RPI_DMA_CHANNEL << 8] = RCIN_RPI_DMA_RESET;                 //Reset DMA
@@ -371,14 +382,14 @@ void RCInput_RPI::init_DMA()
     dma_reg[RCIN_RPI_DMA_CS | RCIN_RPI_DMA_CHANNEL << 8] = RCIN_RPI_DMA_INT | RCIN_RPI_DMA_END;
     dma_reg[RCIN_RPI_DMA_CONBLK_AD | RCIN_RPI_DMA_CHANNEL << 8] = reinterpret_cast<uintptr_t>(con_blocks->get_page(con_blocks->_phys_pages, 0));//Set first control block address
     dma_reg[RCIN_RPI_DMA_DEBUG | RCIN_RPI_DMA_CHANNEL << 8] = 7;                      // clear debug error flags
-    dma_reg[RCIN_RPI_DMA_CS | RCIN_RPI_DMA_CHANNEL << 8] = 0x10880001;                // go, mid priority, wait for outstanding writes    
+    dma_reg[RCIN_RPI_DMA_CS | RCIN_RPI_DMA_CHANNEL << 8] = 0x10880001;                // go, mid priority, wait for outstanding writes
 }
 
 
 //We must stop DMA when the process is killed
 void RCInput_RPI::set_sigaction()
 {
-    for (int i = 0; i < 64; i++) { 
+    for (int i = 0; i < 64; i++) {
         //catch all signals (like ctrl+c, ctrl+z, ...) to ensure DMA is disabled
         struct sigaction sa;
         memset(&sa, 0, sizeof(sa));
@@ -389,26 +400,20 @@ void RCInput_RPI::set_sigaction()
 
 //Initial setup of variables
 RCInput_RPI::RCInput_RPI():
-    prev_tick(0),
-    delta_time(0),
     curr_tick_inc(1000/RCIN_RPI_SAMPLE_FREQ),
     curr_pointer(0),
-    curr_channel(0),
+    curr_channel(0)
-    width_s0(0),
+{
-    curr_signal(0),
-    last_signal(228),
-    state(RCIN_RPI_INITIAL_STATE)
-{    
 #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2
-    int version = 2;    
+    int version = 2;
 #else
     int version = UtilRPI::from(hal.util)->get_rpi_version();
 #endif
     set_physical_addresses(version);
 
-    //Init memory for buffer and for DMA control blocks. See comments in "init_ctrl_data()" to understand values "2" and "113"
+    //Init memory for buffer and for DMA control blocks. See comments in "init_ctrl_data()" to understand values "2" and "15"
     circle_buffer = new Memory_table(RCIN_RPI_BUFFER_LENGTH * 2, version);
-    con_blocks = new Memory_table(RCIN_RPI_BUFFER_LENGTH * 113, version);
+    con_blocks = new Memory_table(RCIN_RPI_BUFFER_LENGTH * 15, version);
 }
 
 RCInput_RPI::~RCInput_RPI()
@@ -425,19 +430,22 @@ void RCInput_RPI::deinit()
 //Initializing necessary registers
 void RCInput_RPI::init_registers()
 {
-    dma_reg = (uint32_t*)map_peripheral(dma_base, RCIN_RPI_DMA_LEN);    
+    dma_reg = (uint32_t*)map_peripheral(dma_base, RCIN_RPI_DMA_LEN);
     pcm_reg = (uint32_t*)map_peripheral(pcm_base, RCIN_RPI_PCM_LEN);
     clk_reg = (uint32_t*)map_peripheral(clk_base, RCIN_RPI_CLK_LEN);
 }
 
 void RCInput_RPI::init()
 {
-    
+    uint64_t signal_states = 0;
+
     init_registers();
-    
+
-    //Enable PPM input
+    //Enable PPM or PWM input
-    enable_pin = hal.gpio->channel(PPM_INPUT_RPI);
+    for (uint32_t i = 0; i < RCIN_RPI_CHN_NUM; ++i) {
-    enable_pin->mode(HAL_GPIO_INPUT);
+        rc_channels[i].enable_pin = hal.gpio->channel(RcChnGpioTbl[i]);
+        rc_channels[i].enable_pin->mode(HAL_GPIO_INPUT);
+    }
 
     //Configuration
     set_sigaction();
@@ -450,11 +458,17 @@ void RCInput_RPI::init()
 
     //Reading first sample
     curr_tick = *((uint64_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer));
-    prev_tick = curr_tick;
     curr_pointer += 8;
-    curr_signal = *((uint8_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer)) & 0x10 ? 1 : 0;
+    signal_states = *((uint64_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer));
-    last_signal = curr_signal;
+    for (uint32_t i = 0; i < RCIN_RPI_CHN_NUM; ++i) {
-    curr_pointer++;
+        rc_channels[i].prev_tick = curr_tick;
+        rc_channels[i].curr_signal = (signal_states & (1 << RcChnGpioTbl[i])) ? RCIN_RPI_SIG_HIGH
+                                                                              : RCIN_RPI_SIG_LOW;
+        rc_channels[i].last_signal = rc_channels[i].curr_signal;
+    }
+    curr_pointer += 8;
+
+    set_num_channels(RCIN_RPI_CHN_NUM);
 }
 
 
@@ -462,16 +476,15 @@ void RCInput_RPI::init()
 void RCInput_RPI::_timer_tick()
 {
     int j;
-    void* x;
+    void *x;
+    uint64_t signal_states(0);
 
     //Now we are getting address in which DMAC is writing at current moment
     dma_cb_t* ad = (dma_cb_t*) con_blocks->get_virt_addr(dma_reg[RCIN_RPI_DMA_CONBLK_AD | RCIN_RPI_DMA_CHANNEL << 8]);
-    for(j = 1; j >= -1; j--){
+    for (j = 1; j >= -1 && x == nullptr; j--) {
-    x = circle_buffer->get_virt_addr((ad + j)->dst);
+        x = circle_buffer->get_virt_addr((ad + j)->dst);
-    if(x != NULL) {
-        break;}
     }
-    
+
     //How many bytes have DMA transfered (and we can process)?
     counter = circle_buffer->bytes_available(curr_pointer, circle_buffer->get_offset(circle_buffer->_virt_pages, (uintptr_t)x));
     //We can't stay in method for a long time, because it may lead to delays
@@ -480,7 +493,7 @@ void RCInput_RPI::_timer_tick()
     }
 
     //Processing ready bytes
-    for (;counter > 0x40;counter--) {
+    for (;counter > 0x40;) {
         //Is it timer samle?
         if (curr_pointer %  (64) == 0) {
             curr_tick = *((uint64_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer));
@@ -488,40 +501,48 @@ void RCInput_RPI::_timer_tick()
             counter-=8;
         }
         //Reading required bit
-        curr_signal = *((uint8_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer)) & 0x10 ? 1 : 0;
+        signal_states = *((uint64_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer));
-        //If the signal changed
+        for (uint32_t i = 0; i < RCIN_RPI_CHN_NUM; ++i) {
-        if (curr_signal != last_signal) {
+            rc_channels[i].curr_signal = (signal_states & (1 << RcChnGpioTbl[i])) ? RCIN_RPI_SIG_HIGH
-            delta_time = curr_tick - prev_tick;
+                                                                                  : RCIN_RPI_SIG_LOW;
-            prev_tick = curr_tick;
+
-            switch (state) {
+            //If the signal changed
-            case RCIN_RPI_INITIAL_STATE:
+            if (rc_channels[i].curr_signal != rc_channels[i].last_signal) {
-                state = RCIN_RPI_ZERO_STATE;
+                rc_channels[i].delta_time = curr_tick - rc_channels[i].prev_tick;
-                break;
+                rc_channels[i].prev_tick = curr_tick;
-            case RCIN_RPI_ZERO_STATE:
+                switch (rc_channels[i].state) {
-                if (curr_signal == 0) {
+                    case RCIN_RPI_INITIAL_STATE:
-                    width_s0 = (uint16_t) delta_time;
+                        rc_channels[i].state = RCIN_RPI_ZERO_STATE;
-                    state = RCIN_RPI_ONE_STATE;
+                        break;
-                    break;
+                    case RCIN_RPI_ZERO_STATE:
+                        if (rc_channels[i].curr_signal == 0) {
+                            rc_channels[i].width_s0 = (uint16_t)rc_channels[i].delta_time;
+                            rc_channels[i].state = RCIN_RPI_ONE_STATE;
+                        }
+                        break;
+                    case RCIN_RPI_ONE_STATE:
+                        if (rc_channels[i].curr_signal == 1) {
+                            rc_channels[i].width_s1 = (uint16_t)rc_channels[i].delta_time;
+                            rc_channels[i].state = RCIN_RPI_ZERO_STATE;
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH
+                            _process_rc_pulse(rc_channels[i].width_s0,
+                                              rc_channels[i].width_s1, i);
+#else
+                            _process_rc_pulse(rc_channels[i].width_s0,
+                                              rc_channels[i].width_s1);
+#endif
+                        }
+                        break;
                 }
-                else 
-                    break;
-            case RCIN_RPI_ONE_STATE:
-                if (curr_signal == 1) {
-                    width_s1 = (uint16_t) delta_time;
-                    state = RCIN_RPI_ZERO_STATE;
-                    _process_rc_pulse(width_s0, width_s1);
-                    break;
-                }
-                else 
-                    break;
             }
+            rc_channels[i].last_signal = rc_channels[i].curr_signal;
         }
-        last_signal = curr_signal;
+        curr_pointer += 8;
-        curr_pointer++;
+        counter -= 8;
         if (curr_pointer >= circle_buffer->get_page_count()*PAGE_SIZE) {
             curr_pointer = 0;
         }
-        curr_tick+=curr_tick_inc;
+        curr_tick += curr_tick_inc;
     }
 }
-#endif // CONFIG_HAL_BOARD_SUBTYPE
+#endif

libraries/AP_HAL_Linux/RCInput_RPI.h

@@ -22,6 +22,11 @@
 #include <assert.h>
 #include <queue>
 
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH
+#define RCIN_RPI_CHN_NUM 8
+#else
+#define RCIN_RPI_CHN_NUM 1
+#endif
 
 enum state_t{
     RCIN_RPI_INITIAL_STATE = -1,
@@ -98,23 +103,33 @@ private:
     Memory_table *con_blocks;
 
     uint64_t curr_tick;
-    uint64_t prev_tick;
-    uint64_t delta_time;
 
     uint32_t curr_tick_inc;
     uint32_t curr_pointer;
     uint32_t curr_channel;
     uint32_t counter;
 
-    uint16_t width_s0;
+    struct RcChannel {
-    uint16_t width_s1;
+        RcChannel() :
+            prev_tick(0), delta_time(0),
+            width_s0(0), width_s1(0),
+            curr_signal(0), last_signal(0),
+            enable_pin(0), state(RCIN_RPI_INITIAL_STATE)
+        {}
 
-    uint8_t curr_signal;
+        uint64_t prev_tick;
-    uint8_t last_signal;
+        uint64_t delta_time;
-  
+
-    state_t state;
+        uint16_t width_s0;
-    
+        uint16_t width_s1;
-    AP_HAL::DigitalSource *enable_pin; 
+
+        uint8_t curr_signal;
+        uint8_t last_signal;
+
+        state_t state;
+
+        AP_HAL::DigitalSource *enable_pin;
+    } rc_channels[RCIN_RPI_CHN_NUM];
 
     void init_dma_cb(dma_cb_t** cbp, uint32_t mode, uint32_t source, uint32_t dest, uint32_t length, uint32_t stride, uint32_t next_cb);
     void* map_peripheral(uint32_t base, uint32_t len);

libraries/AP_HAL_Linux/VideoIn.cpp

@@ -250,29 +250,71 @@ void VideoIn::prepare_capture()
     }
 }
 
+void VideoIn::shrink_8bpp(uint8_t *buffer, uint8_t *new_buffer,
+                          uint32_t width, uint32_t height, uint32_t left,
+                          uint32_t selection_width, uint32_t top,
+                          uint32_t selection_height, uint32_t fx, uint32_t fy)
+{
+    uint32_t i, j, k, kk, px, block_x, block_y, block_position;
+    uint32_t out_width = selection_width / fx;
+    uint32_t out_height = selection_height / fy;
+    uint32_t width_per_fy = width * fy;
+    uint32_t fx_fy = fx * fy;
+    uint32_t width_sum, out_width_sum = 0;
+
+    /* selection offset */
+    block_y = top * width;
+    block_position = left + block_y;
+
+    for (i = 0; i < out_height; i++) {
+        block_x = left;
+        for (j = 0; j < out_width; j++) {
+            px = 0;
+
+            width_sum = 0;
+            for(k = 0; k < fy; k++) {
+                for(kk = 0; kk < fx; kk++) {
+                    px += buffer[block_position + kk + width_sum];
+                }
+                width_sum += width;
+            }
+
+            new_buffer[j + out_width_sum] = px / (fx_fy);
+
+            block_x += fx;
+            block_position = block_x + block_y;
+        }
+        block_y += width_per_fy;
+        out_width_sum += out_width;
+    }
+}
+
 void VideoIn::crop_8bpp(uint8_t *buffer, uint8_t *new_buffer,
                         uint32_t width, uint32_t left, uint32_t crop_width,
                         uint32_t top, uint32_t crop_height)
 {
-    for (uint32_t j = top; j < top + crop_height; j++) {
+    uint32_t crop_x = left + crop_width;
-        for (uint32_t i = left; i < left + crop_width; i++) {
+    uint32_t crop_y = top + crop_height;
-            new_buffer[(i - left) + (j - top) * crop_width] = 
+    uint32_t buffer_index = top * width;
-                buffer[i + j * width];
+    uint32_t new_buffer_index = 0;
+
+    for (uint32_t j = top; j < crop_y; j++) {
+        for (uint32_t i = left; i < crop_x; i++) {
+            new_buffer[i - left + new_buffer_index] =  buffer[i + buffer_index];
         }
+        buffer_index += width;
+        new_buffer_index += crop_width;
     }
 }
 
 void VideoIn::yuyv_to_grey(uint8_t *buffer, uint32_t buffer_size,
                            uint8_t *new_buffer)
 {
-    uint32_t i;
     uint32_t new_buffer_position = 0;
 
-    for (i = 0; i < buffer_size; i++) {
+    for (uint32_t i = 0; i < buffer_size; i += 2) {
-        if (i % 2 == 0) {
+        new_buffer[new_buffer_position] = buffer[i];
-            new_buffer[new_buffer_position] = buffer[i];
+        new_buffer_position++;
-            new_buffer_position++;
-        }
     }
 }
 

libraries/AP_HAL_Linux/VideoIn.h

@@ -51,6 +51,11 @@ public:
                   uint32_t width, uint32_t height);
     void prepare_capture();
 
+    static void shrink_8bpp(uint8_t *buffer, uint8_t *new_buffer,
+                            uint32_t width, uint32_t height, uint32_t left,
+                            uint32_t selection_width, uint32_t top,
+                            uint32_t selection_height, uint32_t fx, uint32_t fy);
+
     static void crop_8bpp(uint8_t *buffer, uint8_t *new_buffer,
                           uint32_t width, uint32_t left,
                           uint32_t crop_width, uint32_t top,

libraries/AP_HAL_Linux/benchmarks/benchmark_videoin.cpp

@@ -0,0 +1,67 @@
+#include <AP_gbenchmark.h>
+#include <AP_HAL/AP_HAL.h>
+
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP ||\
+    CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE
+
+#include <AP_HAL_Linux/VideoIn.h>
+
+static void BM_Crop8bpp(benchmark::State& state)
+{
+    uint8_t *buffer, *new_buffer;
+    uint32_t width = 640;
+    uint32_t height = 480;
+    uint32_t left = width / 2 - state.range_x() / 2;
+    uint32_t top = height / 2 - state.range_y() / 2;
+
+    buffer = (uint8_t *)malloc(width * height);
+    if (!buffer) {
+        fprintf(stderr, "error: couldn't malloc buffer\n");
+        return;
+    }
+
+    new_buffer = (uint8_t *)malloc(state.range_x() * state.range_y());
+    if (!new_buffer) {
+        fprintf(stderr, "error: couldn't malloc new_buffer\n");
+        return;
+    }
+
+    while (state.KeepRunning()) {
+        Linux::VideoIn::crop_8bpp(buffer, new_buffer, width,
+            left, state.range_x(), top, state.range_y());
+    }
+
+    free(buffer);
+    free(new_buffer);
+}
+
+BENCHMARK(BM_Crop8bpp)->ArgPair(64, 64)->ArgPair(240, 240)->ArgPair(640, 480);
+
+static void BM_YuyvToGrey(benchmark::State& state)
+{
+    uint8_t *buffer, *new_buffer;
+
+    buffer = (uint8_t *)malloc(state.range_x());
+    if (!buffer) {
+        fprintf(stderr, "error: couldn't malloc buffer\n");
+        return;
+    }
+
+    new_buffer = (uint8_t *)malloc(state.range_x() / 2);
+    if (!new_buffer) {
+        fprintf(stderr, "error: couldn't malloc new_buffer\n");
+        return;
+    }
+
+    while (state.KeepRunning()) {
+        Linux::VideoIn::yuyv_to_grey(buffer, state.range_x(), new_buffer);
+    }
+
+    free(buffer);
+    free(new_buffer);
+}
+
+BENCHMARK(BM_YuyvToGrey)->Arg(64 * 64)->Arg(320 * 240)->Arg(640 * 480);
+#endif
+
+BENCHMARK_MAIN()

libraries/AP_HAL_Linux/benchmarks/wscript

@@ -0,0 +1,10 @@
+#!/usr/bin/env python
+# encoding: utf-8
+
+import ardupilotwaf
+
+def build(bld):
+    ardupilotwaf.find_benchmarks(
+        bld,
+        use='ap',
+    )

libraries/AP_HAL_Linux/examples/BusTest/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_HAL_PX4/examples/simple/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_InertialSensor/examples/INS_generic/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_InertialSensor/examples/VibTest/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_Math/examples/eulers/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_Math/examples/location/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_Math/examples/polygon/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_Math/examples/rotations/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_Mission/examples/AP_Mission_test/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_Motors/examples/AP_Motors_Time_test/wscript

@@ -7,7 +7,7 @@ def build(bld):
     # TODO: Test code doesn't build. Fix or delete the test.
     return
 
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_Motors/examples/AP_Motors_test/wscript

@@ -7,7 +7,7 @@ def build(bld):
     # TODO: Test code doesn't build. Fix or delete the test.
     return
 
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_Mount/examples/trivial_AP_Mount/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_Notify/examples/AP_Notify_test/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_Notify/examples/ToshibaLED_test/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_Parachute/examples/AP_Parachute_test/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_RangeFinder/examples/RFIND_test/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/AP_Scheduler/examples/Scheduler_test/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/DataFlash/examples/DataFlash_test/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/Filter/examples/Derivative/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )

libraries/Filter/examples/Filter/wscript

@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.example(
         bld,
         use='ap',
     )