Merge remote-tracking branch 'upstream/master'

ArduPlane/arming_checks.cpp

@@ -68,6 +68,13 @@ bool AP_Arming_Plane::pre_arm_checks(bool report)
         ret = false;
     }
 
+    if (plane.control_mode == AUTO && plane.mission.num_commands() <= 1) {
+        if (report) {
+            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: No mission loaded");
+        }
+        ret = false;
+    }
+
     return ret;
 }
 

ArduPlane/system.cpp

@@ -309,7 +309,9 @@ void Plane::startup_ground(void)
     serial_manager.set_blocking_writes_all(false);
 
     ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
-    ins.set_dataflash(&DataFlash);    
+    ins.set_dataflash(&DataFlash);
+
+    GCS_MAVLINK::set_dataflash(&DataFlash);
 
     gcs_send_text(MAV_SEVERITY_INFO,"Ready to fly");
 }

README-WAF.md

@@ -69,7 +69,7 @@ The programs in ardupilot are categorized into the following groups:
  - tools
  - examples: *programs that show how certain libraries are used or to simply
    test their operation*
- - benchmarks
+ - benchmarks: *requires `--enable-benchmarks` during configurarion*
  - tests: *basically unit tests to ensure changes don't break the system's
    logic*
 

Tools/ardupilotwaf/ardupilotwaf.py

@@ -307,7 +307,7 @@ def _select_programs_from_group(bld):
             bld.targets += ',' + tg.name
 
 def options(opt):
-    g = opt.add_option_group('Ardupilot build options')
+    g = opt.ap_groups['build']
     g.add_option('--program-group',
         action='append',
         default=[],
@@ -316,6 +316,11 @@ def options(opt):
              'examples. The special group "all" selects all programs.',
     )
 
+    g = opt.ap_groups['check']
+    g.add_option('--check-verbose',
+                 action='store_true',
+                 help='Output all test programs')
+
 def build(bld):
     global LAST_IDX
     # FIXME: This is done to prevent same task generators being created with

Tools/ardupilotwaf/cmake.py

@@ -0,0 +1,240 @@
+#!/usr/bin/env python
+# encoding: utf-8
+
+# Copyright (C) 2015-2016  Intel Corporation. All rights reserved.
+#
+# This file 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 file 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/>.
+
+"""
+Waf tool for external builds with cmake. This tool defines two features:
+    - cmake_configure: for defining task generators that do cmake build
+      configuration, i.e., build system generation.
+    - cmake_build: for actual build through the cmake interface.
+
+Example::
+
+    def build(bld):
+        # cmake configuration
+        foo = bld(
+            features='cmake_configure',
+            name='foo',
+            cmake_src='path/to/foosrc', # where is the source tree
+            cmake_bld='path/to/foobld', # where to generate the build system
+            cmake_vars=dict(
+                CMAKE_BUILD_TYPE='Release',
+                ...
+            ),
+        )
+
+        # cmake build for target (cmake target) 'bar'
+        bld(
+            features='cmake_build',
+            cmake_config='foo', # this build depends on the cmake generation above defined
+            cmake_target='bar', # what to pass to option --target of cmake
+        )
+
+        # cmake build for target 'baz' (syntactic sugar)
+        foo.cmake_build('baz')
+
+The keys of cmake_vars are sorted so that unnecessary execution is avoided. If
+you want to ensure an order in which the variables are passed to cmake, use an
+OrderedDict. Example::
+
+    def build(bld):
+        foo_vars = OrderedDict()
+        foo_vars['CMAKE_BUILD_TYPE'] = 'Release'
+        foo_vars['FOO'] = 'value_of_foo'
+        foo_vars['BAR'] = 'value_of_bar'
+
+        # cmake configuration
+        foo = bld(
+            features='cmake_configure',
+            cmake_vars=foo_vars,
+            ...
+        )
+
+There may be cases when you want to stablish dependency between other tasks and
+the external build system's products (headers and libraries, for example). In
+that case, you can specify the specific files in the option 'target' of your
+cmake_build task generator. In general, that feature is supposed to be used by
+other tools rather than by wscripts. Example::
+
+    def build(bld):
+        ...
+
+        # declaring on target only what I'm interested in
+        foo.cmake_build('baz', target='path/to/foobld/include/baz.h')
+
+    @feature('myfeature')
+    def process_myfeature(self):
+        baz_taskgen = self.bld.get_tgen_by_name('baz')
+        baz_taskgen.post()
+
+        # every cmake_build taskgen stores its task in cmake_build_task
+        baz_task = baz_taskgen.cmake_build_task
+
+        tsk = self.create_task('mytask')
+
+        tsk.set_run_after(baz_task)
+
+        # tsk is run whenever baz_task changes its outputs, namely,
+        # path/to/foobld/include/baz.h
+        tsk.dep_nodes.extend(baz_task.outputs)
+"""
+
+from waflib import Node, Task, Utils
+from waflib.TaskGen import feature, taskgen_method
+
+from collections import OrderedDict
+import os
+
+class cmake_configure_task(Task.Task):
+    vars = ['CMAKE_BLD_DIR']
+    run_str = '${CMAKE} ${CMAKE_SRC_DIR} ${CMAKE_VARS} ${CMAKE_GENERATOR_OPTION}'
+
+    def uid(self):
+        if not hasattr(self, 'uid_'):
+            m = Utils.md5()
+            def u(s):
+                m.update(s.encode('utf-8'))
+            u(self.__class__.__name__)
+            u(self.env.get_flat('CMAKE_SRC_DIR'))
+            u(self.env.get_flat('CMAKE_BLD_DIR'))
+            self.uid_ = m.digest()
+
+        return self.uid_
+
+    def __str__(self):
+        return self.generator.name
+
+    def keyword(self):
+        return 'CMake Configure'
+
+# Clean cmake configuration
+cmake_configure_task._original_run = cmake_configure_task.run
+def _cmake_configure_task_run(self):
+    cmakecache_path = self.outputs[0].abspath()
+    if os.path.exists(cmakecache_path):
+        os.remove(cmakecache_path)
+    self._original_run()
+cmake_configure_task.run = _cmake_configure_task_run
+
+class cmake_build_task(Task.Task):
+    run_str = '${CMAKE} --build ${CMAKE_BLD_DIR} --target ${CMAKE_TARGET}'
+
+    def uid(self):
+        if not hasattr(self, 'uid_'):
+            m = Utils.md5()
+            def u(s):
+                m.update(s.encode('utf-8'))
+            u(self.__class__.__name__)
+            u(self.env.get_flat('CMAKE_BLD_DIR'))
+            u(self.env.get_flat('CMAKE_TARGET'))
+            self.uid_ = m.digest()
+
+        return self.uid_
+
+    def __str__(self):
+        config_name = self.generator.config_taskgen.name
+        target = self.generator.cmake_target
+        return '%s %s' % (config_name, target)
+
+    def keyword(self):
+        return 'CMake Build'
+
+# allow tasks to depend on possible headers or other resources if the user
+# declares outputs for the cmake build
+cmake_build_task = Task.update_outputs(cmake_build_task)
+
+# the cmake-generated build system is responsible of managing its own
+# dependencies
+cmake_build_task = Task.always_run(cmake_build_task)
+
+@feature('cmake_configure')
+def process_cmake_configure(self):
+    if not hasattr(self, 'name'):
+        self.bld.fatal('cmake_configure: taskgen is missing name')
+    if not hasattr(self, 'cmake_src'):
+        self.bld.fatal('cmake_configure: taskgen is missing cmake_src')
+
+    if not isinstance(self.cmake_src, Node.Node):
+        self.cmake_src = self.bld.path.find_dir(self.cmake_src)
+
+    if not hasattr(self, 'cmake_bld'):
+        self.cmake_bld = self.cmake_src.get_bld()
+    elif not isinstance(self.cmake_bld, Node.Node):
+        self.cmake_bld = self.bld.bldnode.make_node(self.cmake_bld)
+    self.cmake_bld.mkdir()
+
+    self.cmake_vars = getattr(self, 'cmake_vars', {})
+
+    # NOTE: we'll probably need to use the full class name in waf 1.9
+    tsk = self.cmake_config_task = self.create_task('cmake_configure')
+    tsk.cwd = self.cmake_bld.abspath()
+    tsk.env.CMAKE_BLD_DIR = self.cmake_bld.abspath()
+    tsk.env.CMAKE_SRC_DIR = self.cmake_src.abspath()
+
+    keys = list(self.cmake_vars.keys())
+    if not isinstance(self.cmake_vars, OrderedDict):
+        keys.sort()
+    tsk.env.CMAKE_VARS = ["-D%s='%s'" % (k, self.cmake_vars[k]) for k in keys]
+
+    tsk.set_outputs(self.cmake_bld.find_or_declare('CMakeCache.txt'))
+
+@feature('cmake_build')
+def process_cmake_build(self):
+    if not hasattr(self, 'cmake_target'):
+        self.bld.fatal('cmake_build: taskgen is missing cmake_target')
+    if not hasattr(self, 'cmake_config'):
+        self.bld.fatal('cmake_build: taskgen is missing cmake_config')
+
+    self.config_taskgen = self.bld.get_tgen_by_name(self.cmake_config)
+
+    if not getattr(self.config_taskgen, 'posted', False):
+        self.config_taskgen.post()
+
+    # NOTE: we'll probably need to use the full class name in waf 1.9
+    tsk = self.cmake_build_task = self.create_task('cmake_build')
+    tsk.env.CMAKE_BLD_DIR = self.config_taskgen.cmake_bld.abspath()
+    tsk.env.CMAKE_TARGET = self.cmake_target
+    tsk.set_run_after(self.config_taskgen.cmake_config_task)
+
+    outputs = Utils.to_list(getattr(self, 'target', ''))
+    for o in outputs:
+        if not isinstance(o, Node.Node):
+            o = self.path.find_or_declare(o)
+        tsk.set_outputs(o)
+
+@taskgen_method
+def cmake_build(self, cmake_target, **kw):
+    if not 'cmake_configure' in self.features:
+        self.bld.fatal('cmake_build: this is not a cmake_configure taskgen')
+
+    kw['cmake_config'] = self.name
+    kw['cmake_target'] = cmake_target
+
+    if 'name' not in kw:
+        kw['name'] = '%s_%s' % (self.name, cmake_target)
+
+    kw['features'] = Utils.to_list(kw.get('features', []))
+    kw['features'].append('cmake_build')
+
+    return self.bld(**kw)
+
+def configure(cfg):
+    cfg.find_program('cmake')
+    cfg.find_program(['ninja', 'ninja-build'], var='NINJA', mandatory=False)
+    cfg.env.CMAKE_GENERATOR_OPTION = ''
+    if cfg.env.NINJA:
+        cfg.env.CMAKE_GENERATOR_OPTION = '-GNinja'

Tools/ardupilotwaf/gbenchmark.py

@@ -6,37 +6,10 @@ gbenchmark is a Waf tool for benchmark builds in Ardupilot
 """
 
 from waflib import Build, Context, Task
+from waflib.Configure import conf
 from waflib.TaskGen import feature, before_method, after_method
 from waflib.Errors import WafError
 
-def _configure_cmake(ctx, bldnode):
-    env = ctx.env
-    my_build_node = bldnode.find_dir(env.GBENCHMARK_BUILD_REL)
-    if not my_build_node:
-        bldnode.make_node(env.GBENCHMARK_BUILD_REL).mkdir()
-
-    prefix_node = bldnode.make_node(env.GBENCHMARK_PREFIX_REL)
-
-    cmake_vars = {
-        'CMAKE_BUILD_TYPE': 'Release',
-        'CMAKE_INSTALL_PREFIX:PATH': prefix_node.abspath(),
-    }
-
-    cmake_vars = ' '.join("-D%s='%s'" % v for v in cmake_vars.items())
-
-    cmd = '%s %s %s %s' % (
-              env.CMAKE[0],
-              env.GBENCHMARK_SRC,
-              cmake_vars,
-              env.GBENCHMARK_GENERATOR_OPTION
-          )
-
-    ctx.cmd_and_log(
-        cmd,
-        cwd=env.GBENCHMARK_BUILD,
-        quiet=Context.BOTH,
-    )
-
 def configure(cfg):
     env = cfg.env
     env.HAS_GBENCHMARK = False
@@ -49,30 +22,12 @@ def configure(cfg):
         )
         return
 
-    cfg.find_program('cmake', mandatory=False)
+    cfg.load('cmake')
 
-    if not env.CMAKE:
+    env.GBENCHMARK_PREFIX_REL = 'gbenchmark'
-        return
-
-    env.GBENCHMARK_CMAKE_GENERATOR = None
-
-    cfg.find_program(['ninja', 'ninja-build'], var='NINJA', mandatory=False)
-    if env.NINJA:
-        env.GBENCHMARK_CMAKE_GENERATOR = 'Ninja'
-
-    env.GBENCHMARK_GENERATOR_OPTION = ''
-    if env.GBENCHMARK_CMAKE_GENERATOR:
-        env.GBENCHMARK_GENERATOR_OPTION = '-G%s' % env.GBENCHMARK_CMAKE_GENERATOR
 
     bldnode = cfg.bldnode.make_node(cfg.variant)
-    prefix_node = bldnode.make_node('gbenchmark')
+    prefix_node = bldnode.make_node(env.GBENCHMARK_PREFIX_REL)
-    my_build_node = bldnode.make_node('gbenchmark_build')
-    my_src_node = cfg.srcnode.make_node('modules/gbenchmark')
-
-    env.GBENCHMARK_PREFIX_REL = prefix_node.path_from(bldnode)
-    env.GBENCHMARK_BUILD = my_build_node.abspath()
-    env.GBENCHMARK_BUILD_REL = my_build_node.path_from(bldnode)
-    env.GBENCHMARK_SRC = my_src_node.abspath()
 
     env.INCLUDES_GBENCHMARK = [prefix_node.make_node('include').abspath()]
     env.LIBPATH_GBENCHMARK = [prefix_node.make_node('lib').abspath()]
@@ -81,57 +36,32 @@ def configure(cfg):
     env.append_value('GIT_SUBMODULES', 'gbenchmark')
     env.HAS_GBENCHMARK = True
 
-class gbenchmark_build(Task.Task):
+@conf
-    def __init__(self, *k, **kw):
+def libbenchmark(bld):
-        super(gbenchmark_build, self).__init__(*k, **kw)
+    prefix_node = bld.bldnode.make_node(bld.env.GBENCHMARK_PREFIX_REL)
 
-        bldnode = self.generator.bld.bldnode
+    gbenchmark_cmake = bld(
-        output_list = [
+        features='cmake_configure',
-            '%s/%s' % (self.env.GBENCHMARK_PREFIX_REL, path)
+        cmake_src='modules/gbenchmark',
-            for path in (
+        cmake_bld='gbenchmark_build',
-                'include/benchmark/benchmark.h',
+        name='gbenchmark',
-                'include/benchmark/macros.h',
+        cmake_vars=dict(
-                'include/benchmark/benchmark_api.h',
+            CMAKE_BUILD_TYPE='Release',
-                'include/benchmark/reporter.h',
+            CMAKE_INSTALL_PREFIX=prefix_node.abspath(),
-                'lib/libbenchmark.a',
+        )
-            )
+    )
-        ]
-        self.outputs.extend([bldnode.make_node(f) for f in output_list])
 
-    def run(self):
+    prefix_node = bld.bldnode.make_node(bld.env.GBENCHMARK_PREFIX_REL)
-        bld = self.generator.bld
+    output_paths = (
-        cmds = []
+        'lib/libbenchmark.a',
+        'include/benchmark/benchmark.h',
+        'include/benchmark/macros.h',
+        'include/benchmark/benchmark_api.h',
+        'include/benchmark/reporter.h',
+    )
+    outputs = [prefix_node.make_node(path) for path in output_paths]
+    gbenchmark_cmake.cmake_build('install', target=outputs)
 
-        try:
-            # Generate build system first, if necessary
-            my_build_node = bld.bldnode.find_dir(self.env.GBENCHMARK_BUILD_REL)
-            if not (my_build_node and my_build_node.find_resource('CMakeCache.txt')):
-                _configure_cmake(bld, bld.bldnode)
-
-            # Build gbenchmark
-            cmd = '%s --build %s --target install' % (
-                    self.env.CMAKE[0],
-                    self.env.GBENCHMARK_BUILD
-                  )
-            bld.cmd_and_log(
-                cmd,
-                cwd=self.env.GBENCHMARK_BUILD,
-                quiet=Context.BOTH,
-            )
-            return 0
-        except WafError as e:
-            print(e)
-            if hasattr(e, 'stderr'):
-                print('')
-                print(e.stderr)
-            return 1
-
-    def __str__(self):
-        return 'Google Benchmark'
-
-gbenchmark_build = Task.always_run(Task.update_outputs(gbenchmark_build))
-
-build_task = None
 
 @feature('gbenchmark')
 @before_method('process_use')
@@ -142,12 +72,10 @@ def append_gbenchmark_use(self):
 
 @feature('gbenchmark')
 @after_method('process_source')
-def wait_for_gbenchmark_build(self):
+def wait_for_gbenchmark_install(self):
-    global build_task
+    gbenchmark_install = self.bld.get_tgen_by_name('gbenchmark_install')
-
+    gbenchmark_install.post()
-    if not build_task:
-        build_task = self.create_task('gbenchmark_build')
 
     for task in self.compiled_tasks:
-        task.set_run_after(build_task)
+        task.set_run_after(gbenchmark_install.cmake_build_task)
-        task.dep_nodes.extend(build_task.outputs)
+        task.dep_nodes.extend(gbenchmark_install.cmake_build_task.outputs)

Tools/autotest/locations.txt

@@ -21,3 +21,4 @@ Dalby=-27.274439,151.290064,343,8.7
 RFRanch=37.118079,-2.773690,1297.88,180
 KSFO=37.619373,-122.376637,5.3,118
 NFSC=53.637561,9.929800,13,345
+Rotherham=53.275131,-1.19404,21,360

Tools/scripts/build_ci.sh

@@ -41,6 +41,20 @@ build_concurrency=(["navio"]="-j2"
 
 build_extra_clean=(["px4-v2"]="make px4-cleandep")
 
+# special case for SITL testing in CI
+if [ "$CI_BUILD_TARGET" = "sitltest" ]; then
+    echo "Installing pymavlink"
+    git submodule init
+    git submodule update
+    (cd modules/mavlink/pymavlink && python setup.py build install --user)
+    unset BUILDROOT
+    echo "Running SITL QuadCopter test"
+    Tools/autotest/autotest.py -j2 build.ArduCopter fly.ArduCopter
+    echo "Running SITL QuadPlane test"
+    Tools/autotest/autotest.py -j2 build.ArduPlane fly.QuadPlane
+    exit 0
+fi
+
 waf=modules/waf/waf-light
 
 # get list of boards supported by the waf build
@@ -67,7 +81,7 @@ for t in $CI_BUILD_TARGET; do
 
     if [[ -n ${waf_supported_boards[$t]} ]]; then
         echo "Starting waf build for board ${t}..."
-        $waf configure --board $t
+        $waf configure --board $t --enable-benchmarks
         $waf clean
         $waf ${build_concurrency[$t]} all
         if [[ $t == linux ]]; then

Tools/scripts/configure-ci.sh

@@ -4,7 +4,7 @@
 set -ex
 
 PKGS="build-essential gawk ccache genromfs libc6-i386 \
-      python-argparse python-empy python-serial zlib1g-dev gcc-4.9 g++-4.9"
+      python-argparse python-empy python-serial python-pexpect python-dev python-pip zlib1g-dev gcc-4.9 g++-4.9"
 
 ARM_ROOT="gcc-arm-none-eabi-4_9-2015q3"
 ARM_TARBALL="$ARM_ROOT-20150921-linux.tar.bz2"
@@ -15,6 +15,7 @@ RPI_TARBALL="$RPI_ROOT.tar.gz"
 sudo add-apt-repository ppa:ubuntu-toolchain-r/test -y
 sudo apt-get -qq -y update
 sudo apt-get -qq -y install $PKGS
+sudo pip install mavproxy
 sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-4.9 90 \
     --slave /usr/bin/g++ g++ /usr/bin/g++-4.9
 

libraries/AP_Baro/AP_Baro.cpp

@@ -391,7 +391,8 @@ void AP_Baro::update(void)
     for (uint8_t i=0; i<_num_sensors; i++) {
         if (sensors[i].healthy) {
             // update altitude calculation
-            if (is_zero(sensors[i].ground_pressure)) {
+            float ground_pressure = sensors[i].ground_pressure;
+            if (is_zero(ground_pressure) || isnan(ground_pressure) || isinf(ground_pressure)) {
                 sensors[i].ground_pressure = sensors[i].pressure;
             }
             float altitude = sensors[i].altitude;

libraries/AP_HAL/utility/RingBuffer.cpp

@@ -66,6 +66,28 @@ uint32_t ByteBuffer::write(const uint8_t *data, uint32_t len)
     return len;
 }
 
+/*
+  update bytes at the read pointer. Used to update an object without
+  popping it
+ */
+bool ByteBuffer::update(const uint8_t *data, uint32_t len)
+{
+    if (len > available()) {
+        return false;
+    }
+    // perform as two memcpy calls
+    uint32_t n = size - head;
+    if (n > len) {
+        n = len;
+    }
+    memcpy(&buf[head], data, n);
+    data += n;
+    if (len > n) {
+        memcpy(&buf[0], data, len-n);
+    }
+    return true;
+}
+
 bool ByteBuffer::advance(uint32_t n)
 {
     if (n > available()) {
@@ -75,7 +97,10 @@ bool ByteBuffer::advance(uint32_t n)
     return true;
 }
 
-uint32_t ByteBuffer::read(uint8_t *data, uint32_t len)
+/*
+  read len bytes without advancing the read pointer
+ */
+uint32_t ByteBuffer::peekbytes(uint8_t *data, uint32_t len)
 {
     if (len > available()) {
         len = available();
@@ -91,22 +116,22 @@ uint32_t ByteBuffer::read(uint8_t *data, uint32_t len)
 
     // perform first memcpy
     memcpy(data, b, n);
-    advance(n);
     data += n;
 
     if (len > n) {
-        // possible second memcpy
+        // possible second memcpy, must be from front of buffer
-        uint32_t n2;
+        memcpy(data, buf, len-n);
-        b = readptr(n2);
-        if (n2 > len-n) {
-            n2 = len-n;
-        }
-        memcpy(data, b, n2);
-        advance(n2);
     }
     return len;
 }
 
+uint32_t ByteBuffer::read(uint8_t *data, uint32_t len)
+{
+    uint32_t ret = peekbytes(data, len);
+    advance(ret);
+    return ret;
+}
+
 /*
   return a pointer to a contiguous read buffer
  */

libraries/AP_HAL/utility/RingBuffer.h

@@ -30,16 +30,45 @@ class ByteBuffer {
 public:
     ByteBuffer(uint32_t size);
     ~ByteBuffer(void);
-    
+
+    // number of bytes available to be read
     uint32_t available(void) const;
+
+    // number of bytes space available to write
     uint32_t space(void) const;
+
+    // true if available() is zero
     bool empty(void) const;
+
+    // write bytes to ringbuffer. Returns number of bytes written
     uint32_t write(const uint8_t *data, uint32_t len);
+
+    // read bytes from ringbuffer. Returns number of bytes read
     uint32_t read(uint8_t *data, uint32_t len);
+
+    /*
+      update bytes at the read pointer. Used to update an object without
+      popping it
+    */
+    bool update(const uint8_t *data, uint32_t len);
+    
+    // return size of ringbuffer
     uint32_t get_size(void) const { return size; }
+
+    // advance the read pointer (discarding bytes)
     bool advance(uint32_t n);
+
+    // return a pointer to the next available data
     const uint8_t *readptr(uint32_t &available_bytes);
+
+    // peek one byte without advancing read pointer. Return byte
+    // or -1 if none available
     int16_t peek(uint32_t ofs) const;
+
+    /*
+      read len bytes without advancing the read pointer
+    */
+    uint32_t peekbytes(uint8_t *data, uint32_t len);
     
 private:
     uint8_t *buf = nullptr;
@@ -65,21 +94,39 @@ public:
         delete buffer;
     }
 
+    // return number of objects available to be read
     uint32_t available(void) const {
         return buffer->available() / sizeof(T);
     }
+
+    // return number of objects that could be written
     uint32_t space(void) const {
         return buffer->space() / sizeof(T);
     }
+
+    // true is available() == 0
     bool empty(void) const {
         return buffer->empty();
     }
+
+    // push one object
     bool push(const T &object) {
         if (buffer->space() < sizeof(T)) {
             return false;
         }
         return buffer->write((uint8_t*)&object, sizeof(T)) == sizeof(T);
     }
+   
+    /*
+      throw away an object
+     */
+    bool pop(void) {
+        return buffer->advance(sizeof(T));
+    }
+
+    /*
+      pop earliest object off the queue
+     */
     bool pop(T &object) {
         if (buffer->available() < sizeof(T)) {
             return false;
@@ -87,6 +134,31 @@ public:
         return buffer->read((uint8_t*)&object, sizeof(T)) == sizeof(T);
     }
 
+    
+    /*
+     * push_force() is semantically equivalent to:
+     *   if (!push(t)) { pop(); push(t); }
+     */
+    bool push_force(const T &object) {
+        if (buffer->space() < sizeof(T)) {
+            buffer->advance(sizeof(T));
+        }
+        return push(object);
+    }
+
+    /*
+      peek copies an object out without advancing the read pointer
+     */
+    bool peek(T &object) {
+        return peekbytes(&object, sizeof(T)) == sizeof(T);
+    }
+
+    /* update the object at the front of the queue (the one that would
+       be fetched by pop()) */
+    bool update(const T &object) {
+        return buffer->update((uint8_t*)&object, sizeof(T));
+    }
+    
 private:
     ByteBuffer *buffer = nullptr;
     uint32_t size = 0;

libraries/AP_HAL_PX4/Storage.cpp

@@ -52,9 +52,11 @@ uint32_t PX4Storage::_mtd_signature(void)
     if (lseek(mtd_fd, MTD_SIGNATURE_OFFSET, SEEK_SET) != MTD_SIGNATURE_OFFSET) {
         AP_HAL::panic("Failed to seek in " MTD_PARAMS_FILE);
     }
+    bus_lock(true);
     if (read(mtd_fd, &v, sizeof(v)) != sizeof(v)) {
         AP_HAL::panic("Failed to read signature from " MTD_PARAMS_FILE);
     }
+    bus_lock(false);
     close(mtd_fd);
     return v;
 }
@@ -72,9 +74,11 @@ void PX4Storage::_mtd_write_signature(void)
     if (lseek(mtd_fd, MTD_SIGNATURE_OFFSET, SEEK_SET) != MTD_SIGNATURE_OFFSET) {
         AP_HAL::panic("Failed to seek in " MTD_PARAMS_FILE);
     }
+    bus_lock(true);
     if (write(mtd_fd, &v, sizeof(v)) != sizeof(v)) {
         AP_HAL::panic("Failed to write signature in " MTD_PARAMS_FILE);
     }
+    bus_lock(false);
     close(mtd_fd);
 }
 
@@ -104,10 +108,12 @@ void PX4Storage::_upgrade_to_mtd(void)
     }
     close(old_fd);
     ssize_t ret;
+    bus_lock(true);
     if ((ret=::write(mtd_fd, _buffer, sizeof(_buffer))) != sizeof(_buffer)) {
         ::printf("mtd write of %u bytes returned %d errno=%d\n", sizeof(_buffer), ret, errno);
         AP_HAL::panic("Unable to write MTD for upgrade");
     }
+    bus_lock(false);
     close(mtd_fd);
 #if STORAGE_RENAME_OLD_FILE
     rename(OLD_STORAGE_FILE, OLD_STORAGE_FILE_BAK);
@@ -157,7 +163,9 @@ void PX4Storage::_storage_open(void)
 	}
         const uint16_t chunk_size = 128;
         for (uint16_t ofs=0; ofs<sizeof(_buffer); ofs += chunk_size) {
+            bus_lock(true);
             ssize_t ret = read(fd, &_buffer[ofs], chunk_size);
+            bus_lock(false);
             if (ret != chunk_size) {
                 ::printf("storage read of %u bytes at %u to %p failed - got %d errno=%d\n",
                          (unsigned)sizeof(_buffer), (unsigned)ofs, &_buffer[ofs], (int)ret, (int)errno);
@@ -215,6 +223,26 @@ void PX4Storage::write_block(uint16_t loc, const void *src, size_t n)
 	}
 }
 
+void PX4Storage::bus_lock(bool lock)
+{
+#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4)
+    /*
+      this is needed on Pixracer where the ms5611 may be on the same
+      bus as FRAM, and the NuttX ramtron driver does not go via the
+      PX4 spi bus abstraction. The ramtron driver relies on
+      SPI_LOCK(). We need to prevent the ms5611 reads which happen in
+      interrupt context from interfering with the FRAM operations. As
+      the px4 spi bus abstraction just uses interrupt blocking as the
+      locking mechanism we need to block interrupts here as well.
+     */
+    if (lock) {
+        irq_state = irqsave();
+    } else {
+        irqrestore(irq_state);
+    }
+#endif
+}
+
 void PX4Storage::_timer_tick(void)
 {
 	if (!_initialised || _dirty_mask == 0) {
@@ -264,12 +292,15 @@ void PX4Storage::_timer_tick(void)
 	 */
 	if (lseek(_fd, i<<PX4_STORAGE_LINE_SHIFT, SEEK_SET) == (i<<PX4_STORAGE_LINE_SHIFT)) {
 		_dirty_mask &= ~write_mask;
-		if (write(_fd, &_buffer[i<<PX4_STORAGE_LINE_SHIFT], n<<PX4_STORAGE_LINE_SHIFT) != n<<PX4_STORAGE_LINE_SHIFT) {
+                bus_lock(true);
-			// write error - likely EINTR
+		ssize_t ret = write(_fd, &_buffer[i<<PX4_STORAGE_LINE_SHIFT], n<<PX4_STORAGE_LINE_SHIFT);
-			_dirty_mask |= write_mask;
+                bus_lock(false);
-			close(_fd);
+                if (ret != n<<PX4_STORAGE_LINE_SHIFT) {
-			_fd = -1;
+                    // write error - likely EINTR
-			perf_count(_perf_errors);
+                    _dirty_mask |= write_mask;
+                    close(_fd);
+                    _fd = -1;
+                    perf_count(_perf_errors);
 		}
 	}
 	perf_end(_perf_storage);

libraries/AP_HAL_PX4/Storage.h

@@ -37,6 +37,11 @@ private:
     void _upgrade_to_mtd(void);
     uint32_t _mtd_signature(void);
     void _mtd_write_signature(void);
+
+#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4)
+    irqstate_t irq_state;
+#endif
+    void bus_lock(bool lock);
 };
 
 #endif // __AP_HAL_PX4_STORAGE_H__

libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp

@@ -610,38 +610,59 @@ void NavEKF2_core::fuseCompass()
     float q2 = stateStruct.quat[2];
     float q3 = stateStruct.quat[3];
 
+    float magX = magDataDelayed.mag.x;
+    float magY = magDataDelayed.mag.y;
+    float magZ = magDataDelayed.mag.z;
+
     // compass measurement error variance (rad^2)
     const float R_MAG = 3e-2f;
 
-    // Calculate observation Jacobian
+    // calculate intermediate variables for observation jacobian
-    float t2 = q0 * q0;
+    float t2 = q0*q0;
-    float t3 = q1 * q1;
+    float t3 = q1*q1;
-    float t4 = q2 * q2;
+    float t4 = q2*q2;
-    float t5 = q3 * q3;
+    float t5 = q3*q3;
-    float t6 = t2 + t3 - t4 - t5;
+    float t6 = q0*q3*2.0f;
-    float t7 = q0 * q3 * 2.0f;
+    float t8 = t2-t3+t4-t5;
-    float t8 = q1 * q2 * 2.0f;
+    float t9 = q0*q1*2.0f;
-    float t9 = t7 + t8;
+    float t10 = q2*q3*2.0f;
-    float t10;
+    float t11 = t9-t10;
-    if (fabsf(t6) > 1e-6f) {
+    float t14 = q1*q2*2.0f;
-        t10 = 1.0f / (t6 * t6);
+    float t21 = magY*t8;
-    } else  {
+    float t22 = t6+t14;
+    float t23 = magX*t22;
+    float t24 = magZ*t11;
+    float t7 = t21+t23-t24;
+    float t12 = t2+t3-t4-t5;
+    float t13 = magX*t12;
+    float t15 = q0*q2*2.0f;
+    float t16 = q1*q3*2.0f;
+    float t17 = t15+t16;
+    float t18 = magZ*t17;
+    float t19 = t6-t14;
+    float t25 = magY*t19;
+    float t20 = t13+t18-t25;
+    if (fabsf(t20) < 1e-6f) {
         return;
     }
-    float t11 = t9 * t9;
+    float t26 = 1.0f/(t20*t20);
-    float t12 = t10 * t11;
+    float t27 = t7*t7;
-    float t13 = t12 + 1.0f;
+    float t28 = t26*t27;
-    float t14;
+    float t29 = t28+1.0;
-    if (fabsf(t13) > 1e-6f) {
+    if (fabsf(t29) < 1e-12f) {
-        t14 = 1.0f / t13;
-    } else  {
         return;
     }
-    float t15 = 1.0f / t6;
+    float t30 = 1.0f/t29;
+    if (fabsf(t20) < 1e-12f) {
+        return;
+    }
+    float t31 = 1.0f/t20;
+
+    // calculate observation jacobian
     float H_MAG[3];
-    H_MAG[0] = 0.0f;
+    H_MAG[0] = -t30*(t31*(magZ*t8+magY*t11)+t7*t26*(magY*t17+magZ*t19));
-    H_MAG[1] = t14*(t15*(q0*q1*2.0f-q2*q3*2.0f)+t9*t10*(q0*q2*2.0f+q1*q3*2.0f));
+    H_MAG[1] = t30*(t31*(magX*t11+magZ*t22)-t7*t26*(magZ*t12-magX*t17));
-    H_MAG[2] = t14*(t15*(t2-t3+t4-t5)+t9*t10*(t7-t8));
+    H_MAG[2] = t30*(t31*(magX*t8-magY*t22)+t7*t26*(magY*t12+magX*t19));
 
     // Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 3 elements in H are non zero
     float PH[3];
@@ -844,22 +865,20 @@ void NavEKF2_core::FuseDeclination()
 
 }
 
-// Calculate magnetic heading innovation
+// Calculate magnetic heading declination innovation
 float NavEKF2_core::calcMagHeadingInnov()
 {
-    // rotate measured body components into earth axis and compare to declination to give a heading measurement
+    // rotate measured body components into earth axis
-    Vector3f eulerAngles;
-    stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
     Matrix3f Tbn_temp;
-    Tbn_temp.from_euler(eulerAngles.x, eulerAngles.y, 0.0f);
+    stateStruct.quat.rotation_matrix(Tbn_temp);
     Vector3f magMeasNED = Tbn_temp*magDataDelayed.mag;
-    float measHdg = - atan2f(magMeasNED.y,magMeasNED.x) + _ahrs->get_compass()->get_declination();
 
-    // wrap the heading so it sits on the range from +-pi
+    // the observation is the declination angle of the earth field from the compass library
-    measHdg = wrap_PI(measHdg);
+    // the prediction is the azimuth angle of the projection of the measured field onto the horizontal
+    float innovation = atan2f(magMeasNED.y,magMeasNED.x) - _ahrs->get_compass()->get_declination();
 
-    // calculate the innovation and wrap between +-pi
+    // wrap the innovation so it sits on the range from +-pi
-    float innovation = wrap_PI(eulerAngles.z - measHdg);
+    innovation = wrap_PI(innovation);
 
     // Unwrap so that a large yaw gyro bias offset that causes the heading to wrap does not lead to continual uncontrolled heading drift
     if (innovation - lastInnovation > M_PI_F) {

libraries/GCS_MAVLink/GCS.h

@@ -181,6 +181,13 @@ public:
      */
     static bool find_by_mavtype(uint8_t mav_type, uint8_t &sysid, uint8_t &compid, mavlink_channel_t &channel) { return routing.find_by_mavtype(mav_type, sysid, compid, channel); }
 
+    /*
+      set a dataflash pointer for logging
+     */
+    static void set_dataflash(DataFlash_Class *dataflash) {
+        dataflash_p = dataflash;
+    }
+    
 private:
     void        handleMessage(mavlink_message_t * msg);
 
@@ -280,6 +287,9 @@ private:
     // mavlink routing object
     static MAVLink_routing routing;
 
+    // pointer to static dataflash for logging of text messages
+    static DataFlash_Class *dataflash_p;
+    
     // a vehicle can optionally snoop on messages for other systems
     static void (*msg_snoop)(const mavlink_message_t* msg);
 

libraries/GCS_MAVLink/GCS_MAVLink.cpp

@@ -42,6 +42,9 @@ static uint8_t mavlink_locked_mask;
 // routing table
 MAVLink_routing GCS_MAVLINK::routing;
 
+// static dataflash pointer to support logging text messages
+DataFlash_Class *GCS_MAVLINK::dataflash_p;
+
 // snoop function for vehicle types that want to see messages for
 // other targets
 void (*GCS_MAVLINK::msg_snoop)(const mavlink_message_t* msg) = NULL;

mk/PX4/ROMFS/init.d/rc.APM

@@ -211,6 +211,41 @@ else
 fi
 
 
+if mtd start /fs/mtd
+then
+    echo "started mtd driver OK"
+else
+    echo "failed to start mtd driver"
+    echo "failed to start mtd driver" >> $logfile
+    sh /etc/init.d/rc.error           
+fi
+
+if mtd readtest /fs/mtd
+then
+    echo "mtd readtest OK"
+else
+    echo "failed to read mtd"
+    echo "failed to read mtd" >> $logfile
+    sh /etc/init.d/rc.error           
+fi
+
+if [ $BOARD == FMUv2 -o $BOARD == FMUv4 ]
+then
+  # the ramtron on FMUv2 is very fast and can handle trillions of
+  # writes. This full rw test on each boot ensures it is working
+  # properly. We have one board that failed this, so
+  # the test is arguably worth having
+  if mtd rwtest /fs/mtd
+  then
+    echo "mtd rwtest OK"
+  else
+    echo "failed to test mtd"
+    echo "failed to test mtd" >> $logfile
+    sh /etc/init.d/rc.error           
+  fi
+fi
+
+
 echo "Starting APM sensors"
 
 if ms5611 start
@@ -435,24 +470,6 @@ then
     echo "started pwm_input driver"
 fi
 
-if mtd start /fs/mtd
-then
-    echo "started mtd driver OK"
-else
-    echo "failed to start mtd driver"
-    echo "failed to start mtd driver" >> $logfile
-    sh /etc/init.d/rc.error           
-fi
-
-if mtd readtest /fs/mtd
-then
-    echo "mtd readtest OK"
-else
-    echo "failed to read mtd"
-    echo "failed to read mtd" >> $logfile
-    sh /etc/init.d/rc.error           
-fi
-
 # optional oreo leds
 if [ -f /bin/oreoled ]
 then
@@ -474,22 +491,6 @@ then
     echo "irlock started"
 fi
 
-if [ $BOARD == FMUv2 -o $BOARD == FMUv4 ]
-then
-  # the ramtron on FMUv2 is very fast and can handle trillions of
-  # writes. This full rw test on each boot ensures it is working
-  # properly. We have one board that failed this, so
-  # the test is arguably worth having
-  if mtd rwtest /fs/mtd
-  then
-    echo "mtd rwtest OK"
-  else
-    echo "failed to test mtd"
-    echo "failed to test mtd" >> $logfile
-    sh /etc/init.d/rc.error           
-  fi
-fi
-
 echo Starting ArduPilot $deviceA $deviceC $deviceD
 if ArduPilot -d $deviceA -d2 $deviceC -d3 $deviceD start
 then

modules/PX4Firmware

@@ -1 +1 @@
-Subproject commit 8b6cc4d93c9329ffe876493d928951a8f771f253
+Subproject commit 62d935eb1665a74ca9a40ea1438121a594027327

wscript

@@ -39,21 +39,24 @@ def init(ctx):
         c.variant = env.BOARD
 
 def options(opt):
-    opt.load('ardupilotwaf')
-    boards_names = boards.get_boards_names()
-
     opt.load('compiler_cxx compiler_c waf_unit_test python')
-    opt.add_option('--board',
+
+    opt.ap_groups = {
+        'configure': opt.add_option_group('Ardupilot configure options'),
+        'build': opt.add_option_group('Ardupilot build options'),
+        'check': opt.add_option_group('Ardupilot check options'),
+    }
+
+    opt.load('ardupilotwaf')
+
+    g = opt.ap_groups['configure']
+    boards_names = boards.get_boards_names()
+    g.add_option('--board',
                    action='store',
                    choices=boards_names,
                    default='sitl',
                    help='Target board to build, choices are %s' % boards_names)
 
-    g = opt.add_option_group('Check options')
-    g.add_option('--check-verbose',
-                 action='store_true',
-                 help='Output all test programs')
-
     g.add_option('--no-submodule-update',
                  dest='submodule_update',
                  action='store_false',
@@ -61,6 +64,11 @@ def options(opt):
                  help='Don\'t update git submodules. Useful for building ' +
                       'with submodules at specific revisions.')
 
+    g.add_option('--enable-benchmarks',
+                 action='store_true',
+                 default=False,
+                 help='Enable benchmarks')
+
 def configure(cfg):
     cfg.env.BOARD = cfg.options.board
     # use a different variant for each board
@@ -76,7 +84,8 @@ def configure(cfg):
     cfg.load('waf_unit_test')
     cfg.load('mavgen')
     cfg.load('git_submodule')
-    cfg.load('gbenchmark')
+    if cfg.options.enable_benchmarks:
+        cfg.load('gbenchmark')
     cfg.load('gtest')
     cfg.load('static_linking')
 
@@ -157,6 +166,9 @@ def _build_common_taskgens(bld):
 
     bld.libgtest()
 
+    if bld.env.HAS_GBENCHMARK:
+        bld.libbenchmark()
+
 def _build_recursion(bld):
     common_dirs_patterns = [
         # TODO: Currently each vehicle also generate its own copy of the