aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'upstream/master'

Browse Source
This commit is contained in:
Rustom Jehangir 2016-02-23 08:23:51 -08:00
commit ae58a933bb
20 changed files with 583 additions and 206 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduPlane/arming_checks.cpp
View File

@ -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;
}

4
ArduPlane/system.cpp
View File

@ -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");
}

2
README-WAF.md
View File

@ -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*

7
Tools/ardupilotwaf/ardupilotwaf.py
View File

@ -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

240
Tools/ardupilotwaf/cmake.py Normal file
View File

@ -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'

136
Tools/ardupilotwaf/gbenchmark.py
View File

@ -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:
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
env.GBENCHMARK_PREFIX_REL = 'gbenchmark'
bldnode = cfg.bldnode.make_node(cfg.variant)
prefix_node = bldnode.make_node('gbenchmark')
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()
prefix_node = bldnode.make_node(env.GBENCHMARK_PREFIX_REL)
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):
def __init__(self, *k, **kw):
super(gbenchmark_build, self).__init__(*k, **kw)
@conf
def libbenchmark(bld):
prefix_node = bld.bldnode.make_node(bld.env.GBENCHMARK_PREFIX_REL)
bldnode = self.generator.bld.bldnode
output_list = [
'%s/%s' % (self.env.GBENCHMARK_PREFIX_REL, path)
for path in (
'include/benchmark/benchmark.h',
'include/benchmark/macros.h',
'include/benchmark/benchmark_api.h',
'include/benchmark/reporter.h',
'lib/libbenchmark.a',
)
]
self.outputs.extend([bldnode.make_node(f) for f in output_list])
gbenchmark_cmake = bld(
features='cmake_configure',
cmake_src='modules/gbenchmark',
cmake_bld='gbenchmark_build',
name='gbenchmark',
cmake_vars=dict(
CMAKE_BUILD_TYPE='Release',
CMAKE_INSTALL_PREFIX=prefix_node.abspath(),
)
)
def run(self):
bld = self.generator.bld
cmds = []
prefix_node = bld.bldnode.make_node(bld.env.GBENCHMARK_PREFIX_REL)
output_paths = (
'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):
global build_task
if not build_task:
build_task = self.create_task('gbenchmark_build')
def wait_for_gbenchmark_install(self):
gbenchmark_install = self.bld.get_tgen_by_name('gbenchmark_install')
gbenchmark_install.post()
for task in self.compiled_tasks:
task.set_run_after(build_task)
task.dep_nodes.extend(build_task.outputs)
task.set_run_after(gbenchmark_install.cmake_build_task)
task.dep_nodes.extend(gbenchmark_install.cmake_build_task.outputs)

1
Tools/autotest/locations.txt
View File

@ -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

16
Tools/scripts/build_ci.sh
View File

@ -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

3
Tools/scripts/configure-ci.sh
View File

@ -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

3
libraries/AP_Baro/AP_Baro.cpp
View File

@ -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;

45
libraries/AP_HAL/utility/RingBuffer.cpp
View File

@ -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
uint32_t n2;
b = readptr(n2);
if (n2 > len-n) {
n2 = len-n;
}
memcpy(data, b, n2);
advance(n2);
// possible second memcpy, must be from front of buffer
memcpy(data, buf, len-n);
}
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
*/

74
libraries/AP_HAL/utility/RingBuffer.h
View File

@ -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;

43
libraries/AP_HAL_PX4/Storage.cpp
View File

@ -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) {
// write error - likely EINTR
_dirty_mask |= write_mask;
close(_fd);
_fd = -1;
perf_count(_perf_errors);
bus_lock(true);
ssize_t ret = write(_fd, &_buffer[i<<PX4_STORAGE_LINE_SHIFT], n<<PX4_STORAGE_LINE_SHIFT);
bus_lock(false);
if (ret != n<<PX4_STORAGE_LINE_SHIFT) {
// write error - likely EINTR
_dirty_mask |= write_mask;
close(_fd);
_fd = -1;
perf_count(_perf_errors);
}
}
perf_end(_perf_storage);

5
libraries/AP_HAL_PX4/Storage.h
View File

@ -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__

87
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -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
float t2 = q0 * q0;
float t3 = q1 * q1;
float t4 = q2 * q2;
float t5 = q3 * q3;
float t6 = t2 + t3 - t4 - t5;
float t7 = q0 * q3 * 2.0f;
float t8 = q1 * q2 * 2.0f;
float t9 = t7 + t8;
float t10;
if (fabsf(t6) > 1e-6f) {
t10 = 1.0f / (t6 * t6);
} else {
// calculate intermediate variables for observation jacobian
float t2 = q0*q0;
float t3 = q1*q1;
float t4 = q2*q2;
float t5 = q3*q3;
float t6 = q0*q3*2.0f;
float t8 = t2-t3+t4-t5;
float t9 = q0*q1*2.0f;
float t10 = q2*q3*2.0f;
float t11 = t9-t10;
float t14 = q1*q2*2.0f;
float t21 = magY*t8;
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 t12 = t10 * t11;
float t13 = t12 + 1.0f;
float t14;
if (fabsf(t13) > 1e-6f) {
t14 = 1.0f / t13;
} else {
float t26 = 1.0f/(t20*t20);
float t27 = t7*t7;
float t28 = t26*t27;
float t29 = t28+1.0;
if (fabsf(t29) < 1e-12f) {
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[1] = t14*(t15*(q0*q1*2.0f-q2*q3*2.0f)+t9*t10*(q0*q2*2.0f+q1*q3*2.0f));
H_MAG[2] = t14*(t15*(t2-t3+t4-t5)+t9*t10*(t7-t8));
H_MAG[0] = -t30*(t31*(magZ*t8+magY*t11)+t7*t26*(magY*t17+magZ*t19));
H_MAG[1] = t30*(t31*(magX*t11+magZ*t22)-t7*t26*(magZ*t12-magX*t17));
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
Vector3f eulerAngles;
stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
// rotate measured body components into earth axis
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
measHdg = wrap_PI(measHdg);
// the observation is the declination angle of the earth field from the compass library
// 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
float innovation = wrap_PI(eulerAngles.z - measHdg);
// wrap the innovation so it sits on the range from +-pi
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) {

10
libraries/GCS_MAVLink/GCS.h
View File

@ -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);

3
libraries/GCS_MAVLink/GCS_MAVLink.cpp
View File

@ -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;

69
mk/PX4/ROMFS/init.d/rc.APM
View File

@ -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

2
modules/PX4Firmware

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

32
wscript
View File

@ -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