Merge remote-tracking branch 'px4/master' into pwm_ioctls

Conflicts:
	src/drivers/px4io/px4io.cpp

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

@@ -33,6 +33,8 @@ then
 	param set FW_T_SINK_MIN 4.0
 	param set FW_Y_ROLLFF 1.1
 	param set FW_L1_PERIOD 16
+	param set RC_SCALE_ROLL 1.0
+	param set RC_SCALE_PITCH 1.0
 
 	param set SYS_AUTOCONFIG 0
 	param save

ROMFS/px4fmu_common/init.d/100_mpx_easystar

@@ -29,6 +29,8 @@ then
 	param set FW_T_SINK_MIN 4.0
 	param set FW_Y_ROLLFF 1.1
 	param set FW_L1_PERIOD 16
+	param set RC_SCALE_ROLL 1.0
+	param set RC_SCALE_PITCH 1.0
 	
 	param set SYS_AUTOCONFIG 0
 	param save
@@ -98,8 +100,7 @@ else
 	mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix
 fi
 fw_att_control start
-# Not ready yet for prime-time
-#fw_pos_control_l1 start
+fw_pos_control_l1 start
 
 if [ $EXIT_ON_END == yes ]
 then

ROMFS/px4fmu_common/init.d/101_hk_bixler

@@ -29,6 +29,8 @@ then
 	param set FW_T_SINK_MIN 4.0
 	param set FW_Y_ROLLFF 1.1
 	param set FW_L1_PERIOD 16
+	param set RC_SCALE_ROLL 1.0
+	param set RC_SCALE_PITCH 1.0
 	
 	param set SYS_AUTOCONFIG 0
 	param save
@@ -91,8 +93,7 @@ att_pos_estimator_ekf start
 #
 mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix
 fw_att_control start
-# Not ready yet for prime-time
-#fw_pos_control_l1 start
+fw_pos_control_l1 start
 
 if [ $EXIT_ON_END == yes ]
 then

ROMFS/px4fmu_common/init.d/10_dji_f330

@@ -10,12 +10,12 @@ then
 	# Set all params here, then disable autoconfig
 	param set SYS_AUTOCONFIG 0
 
-	param set MC_ATTRATE_D 0.002
+	param set MC_ATTRATE_D 0.004
 	param set MC_ATTRATE_I 0.0
-	param set MC_ATTRATE_P 0.09
+	param set MC_ATTRATE_P 0.12
 	param set MC_ATT_D 0.0
 	param set MC_ATT_I 0.0
-	param set MC_ATT_P 6.8
+	param set MC_ATT_P 7.0
 	param set MC_YAWPOS_D 0.0
 	param set MC_YAWPOS_I 0.0
 	param set MC_YAWPOS_P 2.0

ROMFS/px4fmu_common/init.d/16_3dr_iris

@@ -10,12 +10,12 @@ then
 	# Set all params here, then disable autoconfig
 	param set SYS_AUTOCONFIG 0
 
-	param set MC_ATTRATE_D 0.006
+	param set MC_ATTRATE_D 0.004
 	param set MC_ATTRATE_I 0.0
-	param set MC_ATTRATE_P 0.17
+	param set MC_ATTRATE_P 0.13
 	param set MC_ATT_D 0.0
 	param set MC_ATT_I 0.0
-	param set MC_ATT_P 5.0
+	param set MC_ATT_P 9.0
 	param set MC_YAWPOS_D 0.0
 	param set MC_YAWPOS_I 0.15
 	param set MC_YAWPOS_P 0.5

ROMFS/px4fmu_common/init.d/31_io_phantom

@@ -29,6 +29,8 @@ then
 	param set FW_T_SINK_MIN 4.0
 	param set FW_Y_ROLLFF 1.1
 	param set FW_L1_PERIOD 17
+	param set RC_SCALE_ROLL 1.0
+	param set RC_SCALE_PITCH 1.0
 	
 	param set SYS_AUTOCONFIG 0
 	param save

Tools/px4params/README.md

@@ -0,0 +1,9 @@
+h1. PX4 Parameters Processor
+
+It's designed to scan PX4 source codes, find declarations of tunable parameters,
+and generate the list in various formats.
+
+Currently supported formats are:
+
+* XML for the parametric UI generator
+* Human-readable description in DokuWiki format

Tools/px4params/dokuwikiout.py

@@ -0,0 +1,27 @@
+import output
+
+class DokuWikiOutput(output.Output):
+    def Generate(self, groups):
+        result = ""
+        for group in groups:
+            result += "==== %s ====\n\n" % group.GetName()
+            for param in group.GetParams():
+                code = param.GetFieldValue("code")
+                name = param.GetFieldValue("short_desc")
+                if code != name:
+                    name = "%s (%s)" % (name, code)
+                result += "=== %s ===\n\n" % name
+                long_desc = param.GetFieldValue("long_desc")
+                if long_desc is not None:
+                    result += "%s\n\n" % long_desc
+                min_val = param.GetFieldValue("min")
+                if min_val is not None:
+                    result += "* Minimal value: %s\n" % min_val
+                max_val = param.GetFieldValue("max")
+                if max_val is not None:
+                    result += "* Maximal value: %s\n" % max_val
+                def_val = param.GetFieldValue("default")
+                if def_val is not None:
+                    result += "* Default value: %s\n" % def_val
+                result += "\n"
+        return result

Tools/px4params/output.py

@@ -0,0 +1,5 @@
+class Output(object):
+    def Save(self, groups, fn):
+        data = self.Generate(groups)
+        with open(fn, 'w') as f:
+            f.write(data)

Tools/px4params/parser.py

@@ -0,0 +1,220 @@
+import sys
+import re
+
+class ParameterGroup(object):
+    """
+    Single parameter group
+    """
+    def __init__(self, name):
+        self.name = name
+        self.params = []
+
+    def AddParameter(self, param):
+        """
+        Add parameter to the group
+        """
+        self.params.append(param)
+
+    def GetName(self):
+        """
+        Get parameter group name
+        """
+        return self.name
+
+    def GetParams(self):
+        """
+        Returns the parsed list of parameters. Every parameter is a Parameter
+        object. Note that returned object is not a copy. Modifications affect
+        state of the parser.
+        """
+        return sorted(self.params,
+                cmp=lambda x, y: cmp(x.GetFieldValue("code"),
+                    y.GetFieldValue("code")))
+
+class Parameter(object):
+    """
+    Single parameter
+    """
+
+    # Define sorting order of the fields
+    priority = {
+        "code": 10,
+        "type": 9,
+        "short_desc": 8,
+        "long_desc": 7,
+        "default": 6,
+        "min": 5,
+        "max": 4,
+        # all others == 0 (sorted alphabetically)
+    }
+
+    def __init__(self):
+        self.fields = {}
+
+    def SetField(self, code, value):
+        """
+        Set named field value
+        """
+        self.fields[code] = value
+
+    def GetFieldCodes(self):
+        """
+        Return list of existing field codes in convenient order
+        """
+        return sorted(self.fields.keys(),
+                cmp=lambda x, y: cmp(self.priority.get(y, 0),
+                    self.priority.get(x, 0)) or cmp(x, y))
+
+    def GetFieldValue(self, code):
+        """
+        Return value of the given field code or None if not found.
+        """
+        return self.fields.get(code)
+
+class Parser(object):
+    """
+    Parses provided data and stores all found parameters internally.
+    """
+
+    re_split_lines = re.compile(r'[\r\n]+')
+    re_comment_start = re.compile(r'^\/\*\*')
+    re_comment_content = re.compile(r'^\*\s*(.*)') 
+    re_comment_tag = re.compile(r'@([a-zA-Z][a-zA-Z0-9_]*)\s*(.*)')
+    re_comment_end = re.compile(r'(.*?)\s*\*\/')
+    re_parameter_definition = re.compile(r'PARAM_DEFINE_([A-Z_][A-Z0-9_]*)\s*\(([A-Z_][A-Z0-9_]*)\s*,\s*([^ ,\)]+)\s*\)\s*;')
+    re_cut_type_specifier = re.compile(r'[a-z]+$')
+    re_is_a_number = re.compile(r'^-?[0-9\.]')
+    re_remove_dots = re.compile(r'\.+$')
+
+    valid_tags = set(["min", "max", "group"])
+
+    # Order of parameter groups
+    priority = {
+        # All other groups = 0 (sort alphabetically)
+        "Miscellaneous": -10
+    }
+
+    def __init__(self):
+        self.param_groups = {}
+
+    def GetSupportedExtensions(self):
+        """
+        Returns list of supported file extensions that can be parsed by this
+        parser.
+        """
+        return ["cpp", "c"]
+
+    def Parse(self, contents):
+        """
+        Incrementally parse program contents and append all found parameters
+        to the list.
+        """
+        # This code is essentially a comment-parsing grammar. "state"
+        # represents parser state. It contains human-readable state
+        # names.
+        state = None
+        for line in self.re_split_lines.split(contents):
+            line = line.strip()
+            # Ignore empty lines
+            if line == "":
+                continue
+            if self.re_comment_start.match(line):
+                state = "wait-short"
+                short_desc = None
+                long_desc = None
+                tags = {}
+            elif state is not None and state != "comment-processed":
+                m = self.re_comment_end.search(line)
+                if m:
+                    line = m.group(1)
+                    last_comment_line = True
+                else:
+                    last_comment_line = False
+                m = self.re_comment_content.match(line)
+                if m:
+                    comment_content = m.group(1)
+                    if comment_content == "":
+                        # When short comment ends with empty comment line,
+                        # start waiting for the next part - long comment.
+                        if state == "wait-short-end":
+                            state = "wait-long"
+                    else:
+                        m = self.re_comment_tag.match(comment_content)
+                        if m:
+                            tag, desc = m.group(1, 2)
+                            tags[tag] = desc
+                            current_tag = tag
+                            state = "wait-tag-end"
+                        elif state == "wait-short":
+                            # Store first line of the short description
+                            short_desc = comment_content
+                            state = "wait-short-end"
+                        elif state == "wait-short-end":
+                            # Append comment line to the short description
+                            short_desc += "\n" + comment_content
+                        elif state == "wait-long":
+                            # Store first line of the long description
+                            long_desc = comment_content
+                            state = "wait-long-end"
+                        elif state == "wait-long-end":
+                            # Append comment line to the long description
+                            long_desc += "\n" + comment_content
+                        elif state == "wait-tag-end":
+                            # Append comment line to the tag text
+                            tags[current_tag] += "\n" + comment_content
+                        else:
+                            raise AssertionError(
+                                    "Invalid parser state: %s" % state)
+                elif not last_comment_line:
+                    # Invalid comment line (inside comment, but not starting with
+                    # "*" or "*/". Reset parsed content.
+                    state = None
+                if last_comment_line:
+                    state = "comment-processed"
+            else:
+                # Non-empty line outside the comment
+                m = self.re_parameter_definition.match(line)
+                if m:
+                    tp, code, defval = m.group(1, 2, 3)
+                    # Remove trailing type specifier from numbers: 0.1f => 0.1
+                    if self.re_is_a_number.match(defval):
+                        defval = self.re_cut_type_specifier.sub('', defval)
+                    param = Parameter()
+                    param.SetField("code", code)
+                    param.SetField("short_desc", code)
+                    param.SetField("type", tp)
+                    param.SetField("default", defval)
+                    # If comment was found before the parameter declaration,
+                    # inject its data into the newly created parameter.
+                    group = "Miscellaneous"
+                    if state == "comment-processed":
+                        if short_desc is not None:
+                            param.SetField("short_desc",
+                                    self.re_remove_dots.sub('', short_desc))
+                        if long_desc is not None:
+                            param.SetField("long_desc", long_desc)
+                        for tag in tags:
+                            if tag == "group":
+                                group = tags[tag]
+                            elif tag not in self.valid_tags:
+                                sys.stderr.write("Skipping invalid"
+                                        "documentation tag: '%s'\n" % tag)
+                            else:
+                                param.SetField(tag, tags[tag])
+                    # Store the parameter
+                    if group not in self.param_groups:
+                        self.param_groups[group] = ParameterGroup(group)
+                    self.param_groups[group].AddParameter(param)
+                # Reset parsed comment.
+                state = None
+
+    def GetParamGroups(self):
+        """
+        Returns the parsed list of parameters. Every parameter is a Parameter
+        object. Note that returned object is not a copy. Modifications affect
+        state of the parser.
+        """
+        return sorted(self.param_groups.values(),
+                cmp=lambda x, y: cmp(self.priority.get(y.GetName(), 0),
+                    self.priority.get(x.GetName(), 0)) or cmp(x.GetName(),
+                        y.GetName()))

Tools/px4params/px_process_params.py

@@ -0,0 +1,61 @@
+#!/usr/bin/env python
+############################################################################
+#
+#   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# PX4 paramers processor (main executable file)
+#
+# It scans src/ subdirectory of the project, collects all parameters
+# definitions, and outputs list of parameters in XML and DokuWiki formats.
+#
+
+import scanner
+import parser
+import xmlout
+import dokuwikiout
+
+# Initialize parser
+prs = parser.Parser()
+
+# Scan directories, and parse the files
+sc = scanner.Scanner()
+sc.ScanDir("../../src", prs)
+output = prs.GetParamGroups()
+
+# Output into XML
+out = xmlout.XMLOutput()
+out.Save(output, "parameters.xml")
+
+# Output into DokuWiki
+out = dokuwikiout.DokuWikiOutput()
+out.Save(output, "parameters.wiki")

Tools/px4params/scanner.py

@@ -0,0 +1,34 @@
+import os
+import re
+
+class Scanner(object):
+    """
+    Traverses directory tree, reads all source files, and passes their contents
+    to the Parser.
+    """
+
+    re_file_extension = re.compile(r'\.([^\.]+)$')
+
+    def ScanDir(self, srcdir, parser):
+        """
+        Scans provided path and passes all found contents to the parser using
+        parser.Parse method.
+        """
+        extensions = set(parser.GetSupportedExtensions())
+        for dirname, dirnames, filenames in os.walk(srcdir):
+            for filename in filenames:
+                m = self.re_file_extension.search(filename)
+                if m:
+                    ext = m.group(1)
+                    if ext in extensions:
+                        path = os.path.join(dirname, filename)
+                        self.ScanFile(path, parser)
+
+    def ScanFile(self, path, parser):
+        """
+        Scans provided file and passes its contents to the parser using
+        parser.Parse method.
+        """
+        with open(path, 'r') as f:
+            contents = f.read()
+        parser.Parse(contents)

Tools/px4params/xmlout.py

@@ -0,0 +1,22 @@
+import output
+from xml.dom.minidom import getDOMImplementation
+
+class XMLOutput(output.Output):
+    def Generate(self, groups):
+        impl = getDOMImplementation()
+        xml_document = impl.createDocument(None, "parameters", None)
+        xml_parameters = xml_document.documentElement
+        for group in groups:
+            xml_group = xml_document.createElement("group")
+            xml_group.setAttribute("name", group.GetName())
+            xml_parameters.appendChild(xml_group)
+            for param in group.GetParams():
+                xml_param = xml_document.createElement("parameter")
+                xml_group.appendChild(xml_param)
+                for code in param.GetFieldCodes():
+                    value = param.GetFieldValue(code)
+                    xml_field = xml_document.createElement(code)
+                    xml_param.appendChild(xml_field)
+                    xml_value = xml_document.createTextNode(value)
+                    xml_field.appendChild(xml_value)
+        return xml_document.toprettyxml(indent="    ", newl="\n", encoding="utf-8")

Tools/px_uploader.py

@@ -430,7 +430,7 @@ while True:
                                 # Windows, don't open POSIX ports
                                 if not "/" in port:
                                         up = uploader(port, args.baud)
-                except:
+                except Exception:
                         # open failed, rate-limit our attempts
                         time.sleep(0.05)
 
@@ -443,7 +443,7 @@ while True:
                         up.identify()
                         print("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port))
 
-                except:
+                except Exception:
                         # most probably a timeout talking to the port, no bootloader, try to reboot the board
                         print("attempting reboot on %s..." % port)
                         up.send_reboot()

Tools/tests-host/.gitignore

@@ -0,0 +1,2 @@
+./obj/*
+mixer_test

Tools/tests-host/Makefile

@@ -0,0 +1,39 @@
+
+CC=g++
+CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers -I../../src -D__EXPORT="" -Dnullptr="0"
+
+ODIR=obj
+LDIR =../lib
+
+LIBS=-lm
+
+#_DEPS = test.h
+#DEPS = $(patsubst %,$(IDIR)/%,$(_DEPS))
+
+_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o mixer.o mixer_group.o mixer_load.o
+OBJ = $(patsubst %,$(ODIR)/%,$(_OBJ))
+
+#$(DEPS)
+$(ODIR)/%.o: %.cpp
+	$(CC) -c -o $@ $< $(CFLAGS)
+
+$(ODIR)/%.o: ../../src/systemcmds/tests/%.cpp
+	$(CC) -c -o $@ $< $(CFLAGS)
+
+$(ODIR)/%.o: ../../src/modules/systemlib/%.cpp
+	$(CC) -c -o $@ $< $(CFLAGS)
+
+$(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.cpp
+	$(CC) -c -o $@ $< $(CFLAGS)
+
+$(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.c
+	$(CC) -c -o $@ $< $(CFLAGS)
+
+#
+mixer_test: $(OBJ)
+	g++ -o $@ $^ $(CFLAGS) $(LIBS)
+
+.PHONY: clean
+
+clean:
+	rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~

Tools/tests-host/mixer_test.cpp

@@ -0,0 +1,12 @@
+#include <systemlib/mixer/mixer.h>
+#include <systemlib/err.h>
+#include "../../src/systemcmds/tests/tests.h"
+
+int main(int argc, char *argv[]) {
+	warnx("Host execution started");
+
+	char* args[] = {argv[0], "../../ROMFS/px4fmu_common/mixers/IO_pass.mix",
+				 "../../ROMFS/px4fmu_common/mixers/FMU_quad_w.mix"};
+
+	test_mixer(3, args);
+}

makefiles/config_px4fmu-v1_default.mk

@@ -134,7 +134,10 @@ MODULES		+= lib/geo
 
 # Tutorial code from
 # https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control
-MODULES			+= examples/fixedwing_control
+#MODULES			+= examples/fixedwing_control
+
+# Hardware test
+#MODULES			+= examples/hwtest
 
 #
 # Transitional support - add commands from the NuttX export archive.

makefiles/config_px4fmu-v2_default.mk

@@ -128,8 +128,12 @@ MODULES		+= lib/geo
 # https://pixhawk.ethz.ch/px4/dev/debug_values
 #MODULES		+= examples/px4_mavlink_debug
 
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control
+#MODULES			+= examples/fixedwing_control
+
 # Hardware test
-MODULES			+= examples/hwtest
+#MODULES			+= examples/hwtest
 
 #
 # Transitional support - add commands from the NuttX export archive.

mavlink/include/mavlink/v1.0/mavlink_conversions.h

@@ -9,6 +9,10 @@
 #endif
 #include <math.h>
 
+#ifndef M_PI_2
+    #define M_PI_2 ((float)asin(1))
+#endif
+
 /**
  * @file mavlink_conversions.h
  *
@@ -49,12 +53,12 @@ MAVLINK_HELPER void mavlink_dcm_to_euler(const float dcm[3][3], float* roll, flo
     float phi, theta, psi;
     theta = asin(-dcm[2][0]);
 
-    if (fabs(theta - M_PI_2) < 1.0e-3f) {
+    if (fabsf(theta - (float)M_PI_2) < 1.0e-3f) {
         phi = 0.0f;
-        psi = (atan2(dcm[1][2] - dcm[0][1],
+        psi = (atan2f(dcm[1][2] - dcm[0][1],
                 dcm[0][2] + dcm[1][1]) + phi);
 
-    } else if (fabs(theta + M_PI_2) < 1.0e-3f) {
+    } else if (fabsf(theta + (float)M_PI_2) < 1.0e-3f) {
         phi = 0.0f;
         psi = atan2f(dcm[1][2] - dcm[0][1],
                   dcm[0][2] + dcm[1][1] - phi);
@@ -128,4 +132,4 @@ MAVLINK_HELPER void mavlink_euler_to_dcm(float roll, float pitch, float yaw, flo
     dcm[2][2] = cosPhi * cosThe;
 }
 
-#endif
+#endif

src/drivers/drv_pwm_output.h

@@ -150,6 +150,7 @@ ORB_DECLARE(output_pwm);
 
 #define DSM2_BIND_PULSES 3	/* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */
 #define DSMX_BIND_PULSES 7	/* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */
+#define DSMX8_BIND_PULSES 10	/* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */
 
 /** power up DSM receiver */
 #define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 11)

src/drivers/px4io/px4io_uploader.cpp

@@ -237,7 +237,7 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout)
 	fds[0].fd = _io_fd;
 	fds[0].events = POLLIN;
 
-	/* wait 100 ms for a character */
+	/* wait <timout> ms for a character */
 	int ret = ::poll(&fds[0], 1, timeout);
 
 	if (ret < 1) {

src/drivers/rgbled/rgbled.cpp

@@ -103,6 +103,7 @@ private:
 
 	bool			_running;
 	int			_led_interval;
+	bool			_should_run;
 	int			_counter;
 
 	void 			set_color(rgbled_color_t ledcolor);
@@ -136,6 +137,7 @@ RGBLED::RGBLED(int bus, int rgbled) :
 	_brightness(1.0f),
 	_running(false),
 	_led_interval(0),
+	_should_run(false),
 	_counter(0)
 {
 	memset(&_work, 0, sizeof(_work));
@@ -248,6 +250,11 @@ RGBLED::led_trampoline(void *arg)
 void
 RGBLED::led()
 {
+	if (!_should_run) {
+		_running = false;
+		return;
+	}
+
 	switch (_mode) {
 	case RGBLED_MODE_BLINK_SLOW:
 	case RGBLED_MODE_BLINK_NORMAL:
@@ -409,10 +416,10 @@ RGBLED::set_mode(rgbled_mode_t mode)
 {
 	if (mode != _mode) {
 		_mode = mode;
-		bool should_run = false;
 
 		switch (mode) {
 		case RGBLED_MODE_OFF:
+			_should_run = false;
 			send_led_enable(false);
 			break;
 
@@ -423,7 +430,7 @@ RGBLED::set_mode(rgbled_mode_t mode)
 			break;
 
 		case RGBLED_MODE_BLINK_SLOW:
-			should_run = true;
+			_should_run = true;
 			_counter = 0;
 			_led_interval = 2000;
 			_brightness = 1.0f;
@@ -431,7 +438,7 @@ RGBLED::set_mode(rgbled_mode_t mode)
 			break;
 
 		case RGBLED_MODE_BLINK_NORMAL:
-			should_run = true;
+			_should_run = true;
 			_counter = 0;
 			_led_interval = 500;
 			_brightness = 1.0f;
@@ -439,7 +446,7 @@ RGBLED::set_mode(rgbled_mode_t mode)
 			break;
 
 		case RGBLED_MODE_BLINK_FAST:
-			should_run = true;
+			_should_run = true;
 			_counter = 0;
 			_led_interval = 100;
 			_brightness = 1.0f;
@@ -447,14 +454,14 @@ RGBLED::set_mode(rgbled_mode_t mode)
 			break;
 
 		case RGBLED_MODE_BREATHE:
-			should_run = true;
+			_should_run = true;
 			_counter = 0;
 			_led_interval = 25;
 			send_led_enable(true);
 			break;
 
 		case RGBLED_MODE_PATTERN:
-			should_run = true;
+			_should_run = true;
 			_counter = 0;
 			_brightness = 1.0f;
 			send_led_enable(true);
@@ -466,16 +473,11 @@ RGBLED::set_mode(rgbled_mode_t mode)
 		}
 
 		/* if it should run now, start the workq */
-		if (should_run && !_running) {
+		if (_should_run && !_running) {
 			_running = true;
 			work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1);
 		}
 
-		/* if it should stop, then cancel the workq */
-		if (!should_run && _running) {
-			_running = false;
-			work_cancel(LPWORK, &_work);
-		}
 	}
 }
 

src/lib/ecl/l1/ecl_l1_pos_controller.cpp

@@ -130,8 +130,12 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 	float alongTrackDist = vector_A_to_airplane * vector_AB;
 
 	/* estimate airplane position WRT to B */
-	math::Vector2f vector_B_to_airplane_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized();
-	float bearing_wp_b = atan2f(-vector_B_to_airplane_unit.getY() , -vector_B_to_airplane_unit.getX());
+	math::Vector2f vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized();
+	
+	/* calculate angle of airplane position vector relative to line) */
+
+	// XXX this could probably also be based solely on the dot product
+	float AB_to_BP_bearing = atan2f(vector_B_to_P_unit % vector_AB, vector_B_to_P_unit * vector_AB);
 
 	/* extension from [2], fly directly to A */
 	if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane , 1.0f) < -0.7071f) {
@@ -148,21 +152,30 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 		/* bearing from current position to L1 point */
 		_nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
 
-// XXX this can be useful as last-resort guard, but is currently not needed
-#if 0
-	} else if (absf(bearing_wp_b) > math::radians(80.0f)) {
-		/* extension, fly back to waypoint */
+	/*
+	 * If the AB vector and the vector from B to airplane point in the same
+	 * direction, we have missed the waypoint. At +- 90 degrees we are just passing it.
+	 */
+	} else if (fabsf(AB_to_BP_bearing) < math::radians(100.0f)) {
+		/*
+		 * Extension, fly back to waypoint.
+		 * 
+		 * This corner case is possible if the system was following
+		 * the AB line from waypoint A to waypoint B, then is
+		 * switched to manual mode (or otherwise misses the waypoint)
+		 * and behind the waypoint continues to follow the AB line.
+		 */
 
 		/* calculate eta to fly to waypoint B */
 		
 		/* velocity across / orthogonal to line */
-		xtrack_vel = ground_speed_vector % (-vector_B_to_airplane_unit);
+		xtrack_vel = ground_speed_vector % (-vector_B_to_P_unit);
 		/* velocity along line */
-		ltrack_vel = ground_speed_vector * (-vector_B_to_airplane_unit);
+		ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit);
 		eta = atan2f(xtrack_vel, ltrack_vel);
 		/* bearing from current position to L1 point */
-		_nav_bearing = bearing_wp_b;
-#endif
+		_nav_bearing = atan2f(-vector_B_to_P_unit.getY() , -vector_B_to_P_unit.getX());
+
 	} else {
 
 		/* calculate eta to fly along the line between A and B */

src/modules/att_pos_estimator_ekf/KalmanNav.hpp

@@ -166,19 +166,19 @@ protected:
 	double lat, lon;                   	/**< lat, lon radians */
 	float alt;                   		/**< altitude, meters */
 	// parameters
-	control::BlockParam<float> _vGyro;      /**< gyro process noise */
-	control::BlockParam<float> _vAccel;     /**< accelerometer process noise  */
-	control::BlockParam<float> _rMag;       /**< magnetometer measurement noise  */
-	control::BlockParam<float> _rGpsVel;    /**< gps velocity measurement noise */
-	control::BlockParam<float> _rGpsPos;    /**< gps position measurement noise */
-	control::BlockParam<float> _rGpsAlt;    /**< gps altitude measurement noise */
-	control::BlockParam<float> _rPressAlt;  /**< press altitude measurement noise */
-	control::BlockParam<float> _rAccel;     /**< accelerometer measurement noise */
-	control::BlockParam<float> _magDip;     /**< magnetic inclination with level */
-	control::BlockParam<float> _magDec;     /**< magnetic declination, clockwise rotation */
-	control::BlockParam<float> _g;          /**< gravitational constant */
-	control::BlockParam<float> _faultPos;   /**< fault detection threshold for position */
-	control::BlockParam<float> _faultAtt;   /**< fault detection threshold for attitude */
+	control::BlockParamFloat _vGyro;      /**< gyro process noise */
+	control::BlockParamFloat _vAccel;     /**< accelerometer process noise  */
+	control::BlockParamFloat _rMag;       /**< magnetometer measurement noise  */
+	control::BlockParamFloat _rGpsVel;    /**< gps velocity measurement noise */
+	control::BlockParamFloat _rGpsPos;    /**< gps position measurement noise */
+	control::BlockParamFloat _rGpsAlt;    /**< gps altitude measurement noise */
+	control::BlockParamFloat _rPressAlt;  /**< press altitude measurement noise */
+	control::BlockParamFloat _rAccel;     /**< accelerometer measurement noise */
+	control::BlockParamFloat _magDip;     /**< magnetic inclination with level */
+	control::BlockParamFloat _magDec;     /**< magnetic declination, clockwise rotation */
+	control::BlockParamFloat _g;          /**< gravitational constant */
+	control::BlockParamFloat _faultPos;   /**< fault detection threshold for position */
+	control::BlockParamFloat _faultAtt;   /**< fault detection threshold for attitude */
 	// status
 	bool _attitudeInitialized;
 	bool _positionInitialized;

src/modules/commander/commander.cpp

@@ -1193,31 +1193,19 @@ int commander_thread_main(int argc, char *argv[])
 		bool main_state_changed = check_main_state_changed();
 		bool navigation_state_changed = check_navigation_state_changed();
 
+		hrt_abstime t1 = hrt_absolute_time();
+
+		if (navigation_state_changed || arming_state_changed) {
+			control_mode.flag_armed = armed.armed;	// copy armed state to vehicle_control_mode topic
+		}
+
 		if (arming_state_changed || main_state_changed || navigation_state_changed) {
 			mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state);
 			status_changed = true;
 		}
 
-		hrt_abstime t1 = hrt_absolute_time();
-
-		/* publish arming state */
-		if (arming_state_changed) {
-			armed.timestamp = t1;
-			orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
-		}
-
-		/* publish control mode */
-		if (navigation_state_changed || arming_state_changed) {
-			/* publish new navigation state */
-			control_mode.flag_armed = armed.armed;	// copy armed state to vehicle_control_mode topic
-			control_mode.counter++;
-			control_mode.timestamp = t1;
-			orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
-		}
-
 		/* publish states (armed, control mode, vehicle status) at least with 5 Hz */
 		if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) {
-			status.counter++;
 			status.timestamp = t1;
 			orb_publish(ORB_ID(vehicle_status), status_pub, &status);
 			control_mode.timestamp = t1;

src/modules/commander/state_machine_helper.cpp

@@ -504,7 +504,6 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 	if (valid_transition) {
 		current_status->hil_state = new_state;
 
-		current_status->counter++;
 		current_status->timestamp = hrt_absolute_time();
 		orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
 

src/modules/controllib/block/BlockParam.hpp

@@ -69,22 +69,39 @@ protected:
 /**
  * Parameters that are tied to blocks for updating and nameing.
  */
-template<class T>
-class __EXPORT BlockParam : public BlockParamBase
+
+class __EXPORT BlockParamFloat : public BlockParamBase
 {
 public:
-	BlockParam(Block *block, const char *name, bool parent_prefix=true) :
+	BlockParamFloat(Block *block, const char *name, bool parent_prefix=true) :
 		BlockParamBase(block, name, parent_prefix),
 		_val() {
 		update();
 	}
-	T get() { return _val; }
-	void set(T val) { _val = val; }
+	float get() { return _val; }
+	void set(float val) { _val = val; }
 	void update() {
 		if (_handle != PARAM_INVALID) param_get(_handle, &_val);
 	}
 protected:
-	T _val;
+	float _val;
+};
+
+class __EXPORT BlockParamInt : public BlockParamBase
+{
+public:
+	BlockParamInt(Block *block, const char *name, bool parent_prefix=true) :
+		BlockParamBase(block, name, parent_prefix),
+		_val() {
+		update();
+	}
+	int get() { return _val; }
+	void set(int val) { _val = val; }
+	void update() {
+		if (_handle != PARAM_INVALID) param_get(_handle, &_val);
+	}
+protected:
+	int _val;
 };
 
 } // namespace control

src/modules/controllib/blocks.hpp

@@ -74,8 +74,8 @@ public:
 	float getMax() { return _max.get(); }
 protected:
 // attributes
-	BlockParam<float> _min;
-	BlockParam<float> _max;
+	control::BlockParamFloat _min;
+	control::BlockParamFloat _max;
 };
 
 int __EXPORT blockLimitTest();
@@ -99,7 +99,7 @@ public:
 	float getMax() { return _max.get(); }
 protected:
 // attributes
-	BlockParam<float> _max;
+	control::BlockParamFloat _max;
 };
 
 int __EXPORT blockLimitSymTest();
@@ -126,7 +126,7 @@ public:
 protected:
 // attributes
 	float _state;
-	BlockParam<float> _fCut;
+	control::BlockParamFloat _fCut;
 };
 
 int __EXPORT blockLowPassTest();
@@ -157,7 +157,7 @@ protected:
 // attributes
 	float _u; /**< previous input */
 	float _y; /**< previous output */
-	BlockParam<float> _fCut; /**< cut-off frequency, Hz */
+	control::BlockParamFloat _fCut; /**< cut-off frequency, Hz */
 };
 
 int __EXPORT blockHighPassTest();
@@ -273,7 +273,7 @@ public:
 // accessors
 	float getKP() { return _kP.get(); }
 protected:
-	BlockParam<float> _kP;
+	control::BlockParamFloat _kP;
 };
 
 int __EXPORT blockPTest();
@@ -303,8 +303,8 @@ public:
 	BlockIntegral &getIntegral() { return _integral; }
 private:
 	BlockIntegral _integral;
-	BlockParam<float> _kP;
-	BlockParam<float> _kI;
+	control::BlockParamFloat _kP;
+	control::BlockParamFloat _kI;
 };
 
 int __EXPORT blockPITest();
@@ -334,8 +334,8 @@ public:
 	BlockDerivative &getDerivative() { return _derivative; }
 private:
 	BlockDerivative _derivative;
-	BlockParam<float> _kP;
-	BlockParam<float> _kD;
+	control::BlockParamFloat _kP;
+	control::BlockParamFloat _kD;
 };
 
 int __EXPORT blockPDTest();
@@ -372,9 +372,9 @@ private:
 // attributes
 	BlockIntegral _integral;
 	BlockDerivative _derivative;
-	BlockParam<float> _kP;
-	BlockParam<float> _kI;
-	BlockParam<float> _kD;
+	control::BlockParamFloat _kP;
+	control::BlockParamFloat _kI;
+	control::BlockParamFloat _kD;
 };
 
 int __EXPORT blockPIDTest();
@@ -404,7 +404,7 @@ public:
 	float get() { return _val; }
 private:
 // attributes
-	BlockParam<float> _trim;
+	control::BlockParamFloat _trim;
 	BlockLimit _limit;
 	float _val;
 };
@@ -439,8 +439,8 @@ public:
 	float getMax() { return _max.get(); }
 private:
 // attributes
-	BlockParam<float> _min;
-	BlockParam<float> _max;
+	control::BlockParamFloat _min;
+	control::BlockParamFloat _max;
 };
 
 int __EXPORT blockRandUniformTest();
@@ -486,8 +486,8 @@ public:
 	float getStdDev() { return _stdDev.get(); }
 private:
 // attributes
-	BlockParam<float> _mean;
-	BlockParam<float> _stdDev;
+	control::BlockParamFloat _mean;
+	control::BlockParamFloat _stdDev;
 };
 
 int __EXPORT blockRandGaussTest();

src/modules/mavlink/mavlink_receiver.cpp

@@ -167,6 +167,7 @@ handle_message(mavlink_message_t *msg)
 				/* check if topic is advertised */
 				if (cmd_pub <= 0) {
 					cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
+
 				} else {
 					/* publish */
 					orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
@@ -437,9 +438,11 @@ handle_message(mavlink_message_t *msg)
 
 			if (pub_hil_airspeed < 0) {
 				pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed);
+
 			} else {
 				orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
 			}
+
 			//warnx("SENSOR: IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s);
 
 			/* individual sensor publications */
@@ -455,49 +458,72 @@ handle_message(mavlink_message_t *msg)
 
 			if (pub_hil_gyro < 0) {
 				pub_hil_gyro = orb_advertise(ORB_ID(sensor_gyro), &gyro);
+
 			} else {
 				orb_publish(ORB_ID(sensor_gyro), pub_hil_gyro, &gyro);
 			}
 
 			struct accel_report accel;
+
 			accel.x_raw = imu.xacc / mg2ms2;
+
 			accel.y_raw = imu.yacc / mg2ms2;
+
 			accel.z_raw = imu.zacc / mg2ms2;
+
 			accel.x = imu.xacc;
+
 			accel.y = imu.yacc;
+
 			accel.z = imu.zacc;
+
 			accel.temperature = imu.temperature;
+
 			accel.timestamp = hrt_absolute_time();
 
 			if (pub_hil_accel < 0) {
 				pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel);
+
 			} else {
 				orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel);
 			}
 
 			struct mag_report mag;
+
 			mag.x_raw = imu.xmag / mga2ga;
+
 			mag.y_raw = imu.ymag / mga2ga;
+
 			mag.z_raw = imu.zmag / mga2ga;
+
 			mag.x = imu.xmag;
+
 			mag.y = imu.ymag;
+
 			mag.z = imu.zmag;
+
 			mag.timestamp = hrt_absolute_time();
 
 			if (pub_hil_mag < 0) {
 				pub_hil_mag = orb_advertise(ORB_ID(sensor_mag), &mag);
+
 			} else {
 				orb_publish(ORB_ID(sensor_mag), pub_hil_mag, &mag);
 			}
 
 			struct baro_report baro;
+
 			baro.pressure = imu.abs_pressure;
+
 			baro.altitude = imu.pressure_alt;
+
 			baro.temperature = imu.temperature;
+
 			baro.timestamp = hrt_absolute_time();
 
 			if (pub_hil_baro < 0) {
 				pub_hil_baro = orb_advertise(ORB_ID(sensor_baro), &baro);
+
 			} else {
 				orb_publish(ORB_ID(sensor_baro), pub_hil_baro, &baro);
 			}
@@ -505,6 +531,7 @@ handle_message(mavlink_message_t *msg)
 			/* publish combined sensor topic */
 			if (pub_hil_sensors > 0) {
 				orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
+
 			} else {
 				pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
 			}
@@ -517,6 +544,7 @@ handle_message(mavlink_message_t *msg)
 			/* lazily publish the battery voltage */
 			if (pub_hil_battery > 0) {
 				orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status);
+
 			} else {
 				pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
 			}
@@ -527,7 +555,7 @@ handle_message(mavlink_message_t *msg)
 
 			// output
 			if ((timestamp - old_timestamp) > 10000000) {
-				printf("receiving hil sensor at %d hz\n", hil_frames/10);
+				printf("receiving hil sensor at %d hz\n", hil_frames / 10);
 				old_timestamp = timestamp;
 				hil_frames = 0;
 			}
@@ -552,9 +580,11 @@ handle_message(mavlink_message_t *msg)
 
 			/* gps.cog is in degrees 0..360 * 100, heading is -PI..+PI */
 			float heading_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f;
+
 			/* go back to -PI..PI */
 			if (heading_rad > M_PI_F)
 				heading_rad -= 2.0f * M_PI_F;
+
 			hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m
 			hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m
 			hil_gps.vel_d_m_s = gps.vd * 1e-2f; // from cm to m
@@ -567,6 +597,7 @@ handle_message(mavlink_message_t *msg)
 			/* publish GPS measurement data */
 			if (pub_hil_gps > 0) {
 				orb_publish(ORB_ID(vehicle_gps_position), pub_hil_gps, &hil_gps);
+
 			} else {
 				pub_hil_gps = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
 			}
@@ -585,6 +616,7 @@ handle_message(mavlink_message_t *msg)
 
 			if (pub_hil_airspeed < 0) {
 				pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed);
+
 			} else {
 				orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
 			}
@@ -602,6 +634,7 @@ handle_message(mavlink_message_t *msg)
 
 			if (pub_hil_global_pos > 0) {
 				orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos);
+
 			} else {
 				pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
 			}
@@ -613,8 +646,8 @@ handle_message(mavlink_message_t *msg)
 
 			/* set rotation matrix */
 			for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
-				hil_attitude.R[i][j] = C_nb(i, j);
-			
+					hil_attitude.R[i][j] = C_nb(i, j);
+
 			hil_attitude.R_valid = true;
 
 			/* set quaternion */
@@ -636,22 +669,32 @@ handle_message(mavlink_message_t *msg)
 
 			if (pub_hil_attitude > 0) {
 				orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude);
+
 			} else {
 				pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
 			}
 
 			struct accel_report accel;
+
 			accel.x_raw = hil_state.xacc / 9.81f * 1e3f;
+
 			accel.y_raw = hil_state.yacc / 9.81f * 1e3f;
+
 			accel.z_raw = hil_state.zacc / 9.81f * 1e3f;
+
 			accel.x = hil_state.xacc;
+
 			accel.y = hil_state.yacc;
+
 			accel.z = hil_state.zacc;
+
 			accel.temperature = 25.0f;
+
 			accel.timestamp = hrt_absolute_time();
 
 			if (pub_hil_accel < 0) {
 				pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel);
+
 			} else {
 				orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel);
 			}
@@ -664,6 +707,7 @@ handle_message(mavlink_message_t *msg)
 			/* lazily publish the battery voltage */
 			if (pub_hil_battery > 0) {
 				orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status);
+
 			} else {
 				pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
 			}
@@ -733,17 +777,23 @@ receive_thread(void *arg)
 
 	mavlink_message_t msg;
 
-	prctl(PR_SET_NAME, "mavlink uart rcv", getpid());
+	prctl(PR_SET_NAME, "mavlink_uart_rcv", getpid());
+
+	struct pollfd fds[1];
+	fds[0].fd = uart_fd;
+	fds[0].events = POLLIN;
+
+	ssize_t nread = 0;
 
 	while (!thread_should_exit) {
-
-		struct pollfd fds[1];
-		fds[0].fd = uart_fd;
-		fds[0].events = POLLIN;
-
 		if (poll(fds, 1, timeout) > 0) {
+			if (nread < sizeof(buf)) {
+				/* to avoid reading very small chunks wait for data before reading */
+				usleep(1000);
+			}
+
 			/* non-blocking read. read may return negative values */
-			ssize_t nread = read(uart_fd, buf, sizeof(buf));
+			nread = read(uart_fd, buf, sizeof(buf));
 
 			/* if read failed, this loop won't execute */
 			for (ssize_t i = 0; i < nread; i++) {
@@ -751,10 +801,10 @@ receive_thread(void *arg)
 					/* handle generic messages and commands */
 					handle_message(&msg);
 
-					/* Handle packet with waypoint component */
+					/* handle packet with waypoint component */
 					mavlink_wpm_message_handler(&msg, &global_pos, &local_pos);
 
-					/* Handle packet with parameter component */
+					/* handle packet with parameter component */
 					mavlink_pm_message_handler(MAVLINK_COMM_0, &msg);
 				}
 			}

src/modules/mavlink/orb_listener.c

@@ -711,7 +711,7 @@ static void *
 uorb_receive_thread(void *arg)
 {
 	/* Set thread name */
-	prctl(PR_SET_NAME, "mavlink orb rcv", getpid());
+	prctl(PR_SET_NAME, "mavlink_orb_rcv", getpid());
 
 	/*
 	 * set up poll to block for new data,

src/modules/mavlink_onboard/mavlink.c

@@ -167,12 +167,12 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf
 	case 921600: speed = B921600; break;
 
 	default:
-		fprintf(stderr, "[mavlink] ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud);
+		warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600", baud);
 		return -EINVAL;
 	}
 
 	/* open uart */
-	printf("[mavlink] UART is %s, baudrate is %d\n", uart_name, baud);
+	warnx("UART is %s, baudrate is %d", uart_name, baud);
 	uart = open(uart_name, O_RDWR | O_NOCTTY);
 
 	/* Try to set baud rate */
@@ -183,7 +183,7 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf
 	if (strcmp(uart_name, "/dev/ttyACM0") != OK) {
 		/* Back up the original uart configuration to restore it after exit */
 		if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
-			fprintf(stderr, "[mavlink] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state);
+			warnx("ERROR getting baudrate / termios config for %s: %d", uart_name, termios_state);
 			close(uart);
 			return -1;
 		}
@@ -196,14 +196,14 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf
 
 		/* Set baud rate */
 		if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
-			fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
+			warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)", uart_name, termios_state);
 			close(uart);
 			return -1;
 		}
 
 
 		if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
-			fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
+			warnx("ERROR setting baudrate / termios config for %s (tcsetattr)", uart_name);
 			close(uart);
 			return -1;
 		}
@@ -481,9 +481,9 @@ int mavlink_thread_main(int argc, char *argv[])
 static void
 usage()
 {
-	fprintf(stderr, "usage: mavlink start [-d <devicename>] [-b <baud rate>]\n"
-			"       mavlink stop\n"
-			"       mavlink status\n");
+	fprintf(stderr, "usage: mavlink_onboard start [-d <devicename>] [-b <baud rate>]\n"
+			"       mavlink_onboard stop\n"
+			"       mavlink_onboard status\n");
 	exit(1);
 }
 
@@ -499,7 +499,7 @@ int mavlink_onboard_main(int argc, char *argv[])
 
 		/* this is not an error */
 		if (thread_running)
-			errx(0, "mavlink already running\n");
+			errx(0, "already running");
 
 		thread_should_exit = false;
 		mavlink_task = task_spawn_cmd("mavlink_onboard",
@@ -516,7 +516,7 @@ int mavlink_onboard_main(int argc, char *argv[])
 		while (thread_running) {
 			usleep(200000);
 		}
-		warnx("terminated.");
+		warnx("terminated");
 		exit(0);
 	}
 

src/modules/mavlink_onboard/mavlink_receiver.c

@@ -100,11 +100,11 @@ handle_message(mavlink_message_t *msg)
 		mavlink_msg_command_long_decode(msg, &cmd_mavlink);
 
 		if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid)
-			|| (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
+				|| (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
 			//check for MAVLINK terminate command
 			if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
 				/* This is the link shutdown command, terminate mavlink */
-				printf("[mavlink] Terminating .. \n");
+				warnx("terminating...");
 				fflush(stdout);
 				usleep(50000);
 
@@ -132,6 +132,7 @@ handle_message(mavlink_message_t *msg)
 				if (cmd_pub <= 0) {
 					cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
 				}
+
 				/* publish */
 				orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
 			}
@@ -156,6 +157,7 @@ handle_message(mavlink_message_t *msg)
 		/* check if topic is advertised */
 		if (flow_pub <= 0) {
 			flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
+
 		} else {
 			/* publish */
 			orb_publish(ORB_ID(optical_flow), flow_pub, &f);
@@ -186,6 +188,7 @@ handle_message(mavlink_message_t *msg)
 		/* check if topic is advertised */
 		if (cmd_pub <= 0) {
 			cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
+
 		} else {
 			/* create command */
 			orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
@@ -203,6 +206,7 @@ handle_message(mavlink_message_t *msg)
 
 		if (vicon_position_pub <= 0) {
 			vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
+
 		} else {
 			orb_publish(ORB_ID(vehicle_vicon_position), vicon_position_pub, &vicon_position);
 		}
@@ -219,7 +223,7 @@ handle_message(mavlink_message_t *msg)
 			/* switch to a receiving link mode */
 			gcs_link = false;
 
-			/* 
+			/*
 			 * rate control mode - defined by MAVLink
 			 */
 
@@ -227,33 +231,37 @@ handle_message(mavlink_message_t *msg)
 			bool ml_armed = false;
 
 			switch (quad_motors_setpoint.mode) {
-				case 0:
-					ml_armed = false;
-					break;
-				case 1:
-					ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES;
-					ml_armed = true;
+			case 0:
+				ml_armed = false;
+				break;
 
-					break;
-				case 2:
-					ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE;
-					ml_armed = true;
+			case 1:
+				ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES;
+				ml_armed = true;
 
-					break;
-				case 3:
-					ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY;
-					break;
-				case 4:
-					ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION;
-					break;
+				break;
+
+			case 2:
+				ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE;
+				ml_armed = true;
+
+				break;
+
+			case 3:
+				ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY;
+				break;
+
+			case 4:
+				ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION;
+				break;
 			}
 
-			offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid-1]   / (float)INT16_MAX;
-			offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid-1]  / (float)INT16_MAX;
-			offboard_control_sp.p3= (float)quad_motors_setpoint.yaw[mavlink_system.sysid-1]    / (float)INT16_MAX;
-			offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid-1]/(float)UINT16_MAX;
+			offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1]   / (float)INT16_MAX;
+			offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1]  / (float)INT16_MAX;
+			offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1]    / (float)INT16_MAX;
+			offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX;
 
-			if (quad_motors_setpoint.thrust[mavlink_system.sysid-1] == 0) {
+			if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) {
 				ml_armed = false;
 			}
 
@@ -265,6 +273,7 @@ handle_message(mavlink_message_t *msg)
 			/* check if topic has to be advertised */
 			if (offboard_control_sp_pub <= 0) {
 				offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
+
 			} else {
 				/* Publish */
 				orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp);
@@ -281,31 +290,36 @@ handle_message(mavlink_message_t *msg)
 static void *
 receive_thread(void *arg)
 {
-	int uart_fd = *((int*)arg);
+	int uart_fd = *((int *)arg);
 
 	const int timeout = 1000;
-	uint8_t ch;
+	uint8_t buf[32];
 
 	mavlink_message_t msg;
 
-	prctl(PR_SET_NAME, "mavlink offb rcv", getpid());
+	prctl(PR_SET_NAME, "mavlink_onboard_rcv", getpid());
+
+	struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } };
+
+	ssize_t nread = 0;
 
 	while (!thread_should_exit) {
-
-		struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } };
-
 		if (poll(fds, 1, timeout) > 0) {
-			/* non-blocking read until buffer is empty */
-			int nread = 0;
+			if (nread < sizeof(buf)) {
+				/* to avoid reading very small chunks wait for data before reading */
+				usleep(1000);
+			}
 
-			do {
-				nread = read(uart_fd, &ch, 1);
+			/* non-blocking read. read may return negative values */
+			nread = read(uart_fd, buf, sizeof(buf));
 
-				if (mavlink_parse_char(chan, ch, &msg, &status)) { //parse the char
+			/* if read failed, this loop won't execute */
+			for (ssize_t i = 0; i < nread; i++) {
+				if (mavlink_parse_char(chan, buf[i], &msg, &status)) {
 					/* handle generic messages and commands */
 					handle_message(&msg);
 				}
-			} while (nread > 0);
+			}
 		}
 	}
 
@@ -319,8 +333,8 @@ receive_start(int uart)
 	pthread_attr_init(&receiveloop_attr);
 
 	struct sched_param param;
-  	param.sched_priority = SCHED_PRIORITY_MAX - 40;
-  	(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
+	param.sched_priority = SCHED_PRIORITY_MAX - 40;
+	(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
 
 	pthread_attr_setstacksize(&receiveloop_attr, 2048);
 

src/modules/multirotor_att_control/multirotor_att_control_main.c

@@ -89,18 +89,18 @@ static int
 mc_thread_main(int argc, char *argv[])
 {
 	/* declare and safely initialize all structs */
-	struct vehicle_control_mode_s control_mode;
-	memset(&control_mode, 0, sizeof(control_mode));
 	struct vehicle_attitude_s att;
 	memset(&att, 0, sizeof(att));
 	struct vehicle_attitude_setpoint_s att_sp;
 	memset(&att_sp, 0, sizeof(att_sp));
-	struct manual_control_setpoint_s manual;
-	memset(&manual, 0, sizeof(manual));
-	struct sensor_combined_s raw;
-	memset(&raw, 0, sizeof(raw));
 	struct offboard_control_setpoint_s offboard_sp;
 	memset(&offboard_sp, 0, sizeof(offboard_sp));
+	struct vehicle_control_mode_s control_mode;
+	memset(&control_mode, 0, sizeof(control_mode));
+	struct manual_control_setpoint_s manual;
+	memset(&manual, 0, sizeof(manual));
+	struct sensor_combined_s sensor;
+	memset(&sensor, 0, sizeof(sensor));
 	struct vehicle_rates_setpoint_s rates_sp;
 	memset(&rates_sp, 0, sizeof(rates_sp));
 	struct vehicle_status_s status;
@@ -108,29 +108,16 @@ mc_thread_main(int argc, char *argv[])
 	struct actuator_controls_s actuators;
 	memset(&actuators, 0, sizeof(actuators));
 
-	/* subscribe to attitude, motor setpoints and system state */
-	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	int param_sub = orb_subscribe(ORB_ID(parameter_update));
-	int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
-	int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
-	int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
-	int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
-	int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
-	int status_sub = orb_subscribe(ORB_ID(vehicle_status));
-
-	/*
-	 * Do not rate-limit the loop to prevent aliasing
-	 * if rate-limiting would be desired later, the line below would
-	 * enable it.
-	 *
-	 * rate-limit the attitude subscription to 200Hz to pace our loop
-	 * orb_set_interval(att_sub, 5);
-	 */
-	struct pollfd fds[2] = {
-		{ .fd = att_sub, .events = POLLIN },
-		{ .fd = param_sub, .events = POLLIN }
-	};
+	/* subscribe */
+	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
+	int vehicle_attitude_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
+	int offboard_control_setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
+	int vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
+	int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
+	int vehicle_rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
+	int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
 
 	/* publish actuator controls */
 	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
@@ -146,233 +133,246 @@ mc_thread_main(int argc, char *argv[])
 	perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "multirotor_att_control_interval");
 	perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "multirotor_att_control_err");
 
-	/* welcome user */
 	warnx("starting");
 
 	/* store last control mode to detect mode switches */
 	bool control_yaw_position = true;
 	bool reset_yaw_sp = true;
 
+	struct pollfd fds[1] = {
+		{ .fd = vehicle_attitude_sub, .events = POLLIN },
+	};
+
 	while (!thread_should_exit) {
 
 		/* wait for a sensor update, check for exit condition every 500 ms */
-		int ret = poll(fds, 2, 500);
+		int ret = poll(fds, 1, 500);
 
 		if (ret < 0) {
 			/* poll error, count it in perf */
 			perf_count(mc_err_perf);
 
-		} else if (ret == 0) {
-			/* no return value, ignore */
-		} else {
+		} else if (ret > 0) {
+			/* only run controller if attitude changed */
+			perf_begin(mc_loop_perf);
 
-			/* only update parameters if they changed */
-			if (fds[1].revents & POLLIN) {
-				/* read from param to clear updated flag */
+			/* attitude */
+			orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
+
+			bool updated;
+
+			/* parameters */
+			orb_check(parameter_update_sub, &updated);
+
+			if (updated) {
 				struct parameter_update_s update;
-				orb_copy(ORB_ID(parameter_update), param_sub, &update);
-
+				orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
 				/* update parameters */
 			}
 
-			/* only run controller if attitude changed */
-			if (fds[0].revents & POLLIN) {
+			/* control mode */
+			orb_check(vehicle_control_mode_sub, &updated);
 
-				perf_begin(mc_loop_perf);
+			if (updated) {
+				orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub, &control_mode);
+			}
 
-				/* get a local copy of system state */
-				bool updated;
-				orb_check(control_mode_sub, &updated);
+			/* manual control setpoint */
+			orb_check(manual_control_setpoint_sub, &updated);
 
-				if (updated) {
-					orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
+			if (updated) {
+				orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual);
+			}
+
+			/* attitude setpoint */
+			orb_check(vehicle_attitude_setpoint_sub, &updated);
+
+			if (updated) {
+				orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp);
+			}
+
+			/* offboard control setpoint */
+			orb_check(offboard_control_setpoint_sub, &updated);
+
+			if (updated) {
+				orb_copy(ORB_ID(offboard_control_setpoint), offboard_control_setpoint_sub, &offboard_sp);
+			}
+
+			/* vehicle status */
+			orb_check(vehicle_status_sub, &updated);
+
+			if (updated) {
+				orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &status);
+			}
+
+			/* sensors */
+			orb_check(sensor_combined_sub, &updated);
+
+			if (updated) {
+				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
+			}
+
+			/* set flag to safe value */
+			control_yaw_position = true;
+
+			/* reset yaw setpoint if not armed */
+			if (!control_mode.flag_armed) {
+				reset_yaw_sp = true;
+			}
+
+			/* define which input is the dominating control input */
+			if (control_mode.flag_control_offboard_enabled) {
+				/* offboard inputs */
+				if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
+					rates_sp.roll = offboard_sp.p1;
+					rates_sp.pitch = offboard_sp.p2;
+					rates_sp.yaw = offboard_sp.p3;
+					rates_sp.thrust = offboard_sp.p4;
+					rates_sp.timestamp = hrt_absolute_time();
+					orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
+
+				} else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) {
+					att_sp.roll_body = offboard_sp.p1;
+					att_sp.pitch_body = offboard_sp.p2;
+					att_sp.yaw_body = offboard_sp.p3;
+					att_sp.thrust = offboard_sp.p4;
+					att_sp.timestamp = hrt_absolute_time();
+					/* publish the result to the vehicle actuators */
+					orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
 				}
 
-				/* get a local copy of manual setpoint */
-				orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
-				/* get a local copy of attitude */
-				orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
-				/* get a local copy of attitude setpoint */
-				orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp);
-				/* get a local copy of rates setpoint */
-				orb_check(setpoint_sub, &updated);
+				/* reset yaw setpoint after offboard control */
+				reset_yaw_sp = true;
 
-				if (updated) {
-					orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp);
-				}
+			} else if (control_mode.flag_control_manual_enabled) {
+				/* manual input */
+				if (control_mode.flag_control_attitude_enabled) {
+					/* control attitude, update attitude setpoint depending on mode */
+					if (att_sp.thrust < 0.1f) {
+						/* no thrust, don't try to control yaw */
+						rates_sp.yaw = 0.0f;
+						control_yaw_position = false;
 
-				/* get a local copy of status */
-				orb_check(status_sub, &updated);
-
-				if (updated) {
-					orb_copy(ORB_ID(vehicle_status), status_sub, &status);
-				}
-
-				/* get a local copy of the current sensor values */
-				orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
-
-				/* set flag to safe value */
-				control_yaw_position = true;
-
-				/* reset yaw setpoint if not armed */
-				if (!control_mode.flag_armed) {
-					reset_yaw_sp = true;
-				}
-
-				/* define which input is the dominating control input */
-				if (control_mode.flag_control_offboard_enabled) {
-					/* offboard inputs */
-					if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
-						rates_sp.roll = offboard_sp.p1;
-						rates_sp.pitch = offboard_sp.p2;
-						rates_sp.yaw = offboard_sp.p3;
-						rates_sp.thrust = offboard_sp.p4;
-						rates_sp.timestamp = hrt_absolute_time();
-						orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
-
-					} else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) {
-						att_sp.roll_body = offboard_sp.p1;
-						att_sp.pitch_body = offboard_sp.p2;
-						att_sp.yaw_body = offboard_sp.p3;
-						att_sp.thrust = offboard_sp.p4;
-						att_sp.timestamp = hrt_absolute_time();
-						/* publish the result to the vehicle actuators */
-						orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-					}
-
-					/* reset yaw setpoint after offboard control */
-					reset_yaw_sp = true;
-
-				} else if (control_mode.flag_control_manual_enabled) {
-					/* manual input */
-					if (control_mode.flag_control_attitude_enabled) {
-						/* control attitude, update attitude setpoint depending on mode */
-						if (att_sp.thrust < 0.1f) {
-							/* no thrust, don't try to control yaw */
-							rates_sp.yaw = 0.0f;
-							control_yaw_position = false;
-
-							if (status.condition_landed) {
-								/* reset yaw setpoint if on ground */
-								reset_yaw_sp = true;
-							}
-
-						} else {
-							/* only move yaw setpoint if manual input is != 0 */
-							if (manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) {
-								/* control yaw rate */
-								control_yaw_position = false;
-								rates_sp.yaw = manual.yaw;
-								reset_yaw_sp = true;	// has no effect on control, just for beautiful log
-
-							} else {
-								control_yaw_position = true;
-							}
+						if (status.condition_landed) {
+							/* reset yaw setpoint if on ground */
+							reset_yaw_sp = true;
 						}
 
-						if (!control_mode.flag_control_velocity_enabled) {
-							/* update attitude setpoint if not in position control mode */
-							att_sp.roll_body = manual.roll;
-							att_sp.pitch_body = manual.pitch;
-
-							if (!control_mode.flag_control_climb_rate_enabled) {
-								/* pass throttle directly if not in altitude control mode */
-								att_sp.thrust = manual.throttle;
-							}
-						}
-
-						/* reset yaw setpint to current position if needed */
-						if (reset_yaw_sp) {
-							att_sp.yaw_body = att.yaw;
-							reset_yaw_sp = false;
-						}
-
-						if (motor_test_mode) {
-							printf("testmode");
-							att_sp.roll_body = 0.0f;
-							att_sp.pitch_body = 0.0f;
-							att_sp.yaw_body = 0.0f;
-							att_sp.thrust = 0.1f;
-						}
-
-						att_sp.timestamp = hrt_absolute_time();
-
-						/* publish the attitude setpoint */
-						orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-
 					} else {
-						/* manual rate inputs (ACRO), from RC control or joystick */
-						if (control_mode.flag_control_rates_enabled) {
-							rates_sp.roll = manual.roll;
-							rates_sp.pitch = manual.pitch;
+						/* only move yaw setpoint if manual input is != 0 */
+						if (manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) {
+							/* control yaw rate */
+							control_yaw_position = false;
 							rates_sp.yaw = manual.yaw;
-							rates_sp.thrust = manual.throttle;
-							rates_sp.timestamp = hrt_absolute_time();
-						}
+							reset_yaw_sp = true;	// has no effect on control, just for beautiful log
 
-						/* reset yaw setpoint after ACRO */
-						reset_yaw_sp = true;
+						} else {
+							control_yaw_position = true;
+						}
 					}
 
+					if (!control_mode.flag_control_velocity_enabled) {
+						/* update attitude setpoint if not in position control mode */
+						att_sp.roll_body = manual.roll;
+						att_sp.pitch_body = manual.pitch;
+
+						if (!control_mode.flag_control_climb_rate_enabled) {
+							/* pass throttle directly if not in altitude control mode */
+							att_sp.thrust = manual.throttle;
+						}
+					}
+
+					/* reset yaw setpint to current position if needed */
+					if (reset_yaw_sp) {
+						att_sp.yaw_body = att.yaw;
+						reset_yaw_sp = false;
+					}
+
+					if (motor_test_mode) {
+						printf("testmode");
+						att_sp.roll_body = 0.0f;
+						att_sp.pitch_body = 0.0f;
+						att_sp.yaw_body = 0.0f;
+						att_sp.thrust = 0.1f;
+					}
+
+					att_sp.timestamp = hrt_absolute_time();
+
+					/* publish the attitude setpoint */
+					orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
+
 				} else {
-					if (!control_mode.flag_control_auto_enabled) {
-						/* no control, try to stay on place */
-						if (!control_mode.flag_control_velocity_enabled) {
-							/* no velocity control, reset attitude setpoint */
-							att_sp.roll_body = 0.0f;
-							att_sp.pitch_body = 0.0f;
-							att_sp.timestamp = hrt_absolute_time();
-							orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-						}
+					/* manual rate inputs (ACRO), from RC control or joystick */
+					if (control_mode.flag_control_rates_enabled) {
+						rates_sp.roll = manual.roll;
+						rates_sp.pitch = manual.pitch;
+						rates_sp.yaw = manual.yaw;
+						rates_sp.thrust = manual.throttle;
+						rates_sp.timestamp = hrt_absolute_time();
 					}
 
-					/* reset yaw setpoint after non-manual control */
+					/* reset yaw setpoint after ACRO */
 					reset_yaw_sp = true;
 				}
 
-				/* check if we should we reset integrals */
-				bool reset_integral = !control_mode.flag_armed || att_sp.thrust < 0.1f;	// TODO use landed status instead of throttle
-
-				/* run attitude controller if needed */
-				if (control_mode.flag_control_attitude_enabled) {
-					multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, reset_integral);
-					orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
-				}
-
-				/* measure in what intervals the controller runs */
-				perf_count(mc_interval_perf);
-
-				/* run rates controller if needed */
-				if (control_mode.flag_control_rates_enabled) {
-					/* get current rate setpoint */
-					bool rates_sp_updated = false;
-					orb_check(rates_sp_sub, &rates_sp_updated);
-
-					if (rates_sp_updated) {
-						orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp);
+			} else {
+				if (!control_mode.flag_control_auto_enabled) {
+					/* no control, try to stay on place */
+					if (!control_mode.flag_control_velocity_enabled) {
+						/* no velocity control, reset attitude setpoint */
+						att_sp.roll_body = 0.0f;
+						att_sp.pitch_body = 0.0f;
+						att_sp.timestamp = hrt_absolute_time();
+						orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
 					}
-
-					/* apply controller */
-					float rates[3];
-					rates[0] = att.rollspeed;
-					rates[1] = att.pitchspeed;
-					rates[2] = att.yawspeed;
-					multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral);
-
-				} else {
-					/* rates controller disabled, set actuators to zero for safety */
-					actuators.control[0] = 0.0f;
-					actuators.control[1] = 0.0f;
-					actuators.control[2] = 0.0f;
-					actuators.control[3] = 0.0f;
 				}
 
-				actuators.timestamp = hrt_absolute_time();
-				orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
+				/* reset yaw setpoint after non-manual control */
+				reset_yaw_sp = true;
+			}
 
-				perf_end(mc_loop_perf);
-			} /* end of poll call for attitude updates */
-		} /* end of poll return value check */
+			/* check if we should we reset integrals */
+			bool reset_integral = !control_mode.flag_armed || att_sp.thrust < 0.1f;	// TODO use landed status instead of throttle
+
+			/* run attitude controller if needed */
+			if (control_mode.flag_control_attitude_enabled) {
+				multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, reset_integral);
+				orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
+			}
+
+			/* measure in what intervals the controller runs */
+			perf_count(mc_interval_perf);
+
+			/* run rates controller if needed */
+			if (control_mode.flag_control_rates_enabled) {
+				/* get current rate setpoint */
+				bool rates_sp_updated = false;
+				orb_check(vehicle_rates_setpoint_sub, &rates_sp_updated);
+
+				if (rates_sp_updated) {
+					orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rates_setpoint_sub, &rates_sp);
+				}
+
+				/* apply controller */
+				float rates[3];
+				rates[0] = att.rollspeed;
+				rates[1] = att.pitchspeed;
+				rates[2] = att.yawspeed;
+				multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral);
+
+			} else {
+				/* rates controller disabled, set actuators to zero for safety */
+				actuators.control[0] = 0.0f;
+				actuators.control[1] = 0.0f;
+				actuators.control[2] = 0.0f;
+				actuators.control[3] = 0.0f;
+			}
+
+			actuators.timestamp = hrt_absolute_time();
+			orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
+
+			perf_end(mc_loop_perf);
+		}
 	}
 
 	warnx("stopping, disarming motors");
@@ -383,10 +383,9 @@ mc_thread_main(int argc, char *argv[])
 
 	orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
 
-
-	close(att_sub);
-	close(control_mode_sub);
-	close(manual_sub);
+	close(vehicle_attitude_sub);
+	close(vehicle_control_mode_sub);
+	close(manual_control_setpoint_sub);
 	close(actuator_pub);
 	close(att_sp_pub);
 

src/modules/px4iofirmware/dsm.c

@@ -285,10 +285,10 @@ dsm_bind(uint16_t cmd, int pulses)
 
 		/*Pulse RX pin a number of times*/
 		for (int i = 0; i < pulses; i++) {
+			up_udelay(25);
 			stm32_gpiowrite(usart1RxAsOutp, false);
 			up_udelay(25);
 			stm32_gpiowrite(usart1RxAsOutp, true);
-			up_udelay(25);
 		}
 		break;
 

src/modules/sdlog2/sdlog2.c

@@ -583,6 +583,15 @@ int sdlog2_thread_main(int argc, char *argv[])
 		errx(1, "unable to create logging folder, exiting.");
 	}
 
+	const char *converter_in = "/etc/logging/conv.zip";
+	char* converter_out = malloc(120);
+	sprintf(converter_out, "%s/conv.zip", folder_path);
+
+	if (file_copy(converter_in, converter_out)) {
+		errx(1, "unable to copy conversion scripts, exiting.");
+	}
+	free(converter_out);
+
 	/* only print logging path, important to find log file later */
 	warnx("logging to directory: %s", folder_path);
 
@@ -1251,7 +1260,7 @@ int file_copy(const char *file_old, const char *file_new)
 	fclose(source);
 	fclose(target);
 
-	return ret;
+	return OK;
 }
 
 void handle_command(struct vehicle_command_s *cmd)

src/modules/sensors/sensor_params.c

@@ -44,8 +44,34 @@
 
 #include <systemlib/param/param.h>
 
+/**
+ * Gyro X offset FIXME
+ *
+ * This is an X-axis offset for the gyro.
+ * Adjust it according to the calibration data.
+ *
+ * @min -10.0
+ * @max 10.0
+ * @group Gyro Config
+ */
 PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f);
+
+/**
+ * Gyro Y offset FIXME with dot.
+ *
+ * @min -10.0
+ * @max 10.0
+ * @group Gyro Config
+ */
 PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f);
+
+/**
+ * Gyro Z offset FIXME
+ *
+ * @min -5.0
+ * @max 5.0
+ * @group Gyro Config
+ */
 PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f);
 
 PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f);

src/modules/sensors/sensors.cpp

@@ -68,7 +68,6 @@
 #include <systemlib/err.h>
 #include <systemlib/perf_counter.h>
 
-#include <systemlib/ppm_decode.h>
 #include <systemlib/airspeed.h>
 
 #include <uORB/uORB.h>
@@ -105,22 +104,22 @@
  * IN13 - aux1
  * IN14 - aux2
  * IN15 - pressure sensor
- * 
+ *
  * IO:
  * IN4 - servo supply rail
  * IN5 - analog RSSI
  */
 
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
-  #define ADC_BATTERY_VOLTAGE_CHANNEL	10
-  #define ADC_AIRSPEED_VOLTAGE_CHANNEL	11
+#define ADC_BATTERY_VOLTAGE_CHANNEL	10
+#define ADC_AIRSPEED_VOLTAGE_CHANNEL	11
 #endif
 
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
-  #define ADC_BATTERY_VOLTAGE_CHANNEL	2
-  #define ADC_BATTERY_CURRENT_CHANNEL	3
-  #define ADC_5V_RAIL_SENSE		4
-  #define ADC_AIRSPEED_VOLTAGE_CHANNEL	15
+#define ADC_BATTERY_VOLTAGE_CHANNEL	2
+#define ADC_BATTERY_CURRENT_CHANNEL	3
+#define ADC_5V_RAIL_SENSE		4
+#define ADC_AIRSPEED_VOLTAGE_CHANNEL	15
 #endif
 
 #define BAT_VOL_INITIAL 0.f
@@ -134,8 +133,6 @@
  */
 #define PCB_TEMP_ESTIMATE_DEG 5.0f
 
-#define PPM_INPUT_TIMEOUT_INTERVAL	50000 /**< 50 ms timeout / 20 Hz */
-
 #define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg)
 
 /**
@@ -143,70 +140,68 @@
  * This enum maps from board attitude to airframe attitude.
  */
 enum Rotation {
-    ROTATION_NONE                = 0,
-    ROTATION_YAW_45              = 1,
-    ROTATION_YAW_90              = 2,
-    ROTATION_YAW_135             = 3,
-    ROTATION_YAW_180             = 4,
-    ROTATION_YAW_225             = 5,
-    ROTATION_YAW_270             = 6,
-    ROTATION_YAW_315             = 7,
-    ROTATION_ROLL_180            = 8,
-    ROTATION_ROLL_180_YAW_45     = 9,
-    ROTATION_ROLL_180_YAW_90     = 10,
-    ROTATION_ROLL_180_YAW_135    = 11,
-    ROTATION_PITCH_180           = 12,
-    ROTATION_ROLL_180_YAW_225    = 13,
-    ROTATION_ROLL_180_YAW_270    = 14,
-    ROTATION_ROLL_180_YAW_315    = 15,
-    ROTATION_ROLL_90             = 16,
-    ROTATION_ROLL_90_YAW_45      = 17,
-    ROTATION_ROLL_90_YAW_90      = 18,
-    ROTATION_ROLL_90_YAW_135     = 19,
-    ROTATION_ROLL_270            = 20,
-    ROTATION_ROLL_270_YAW_45     = 21,
-    ROTATION_ROLL_270_YAW_90     = 22,
-    ROTATION_ROLL_270_YAW_135    = 23,
-    ROTATION_PITCH_90            = 24,
-    ROTATION_PITCH_270           = 25,
-    ROTATION_MAX
+	ROTATION_NONE                = 0,
+	ROTATION_YAW_45              = 1,
+	ROTATION_YAW_90              = 2,
+	ROTATION_YAW_135             = 3,
+	ROTATION_YAW_180             = 4,
+	ROTATION_YAW_225             = 5,
+	ROTATION_YAW_270             = 6,
+	ROTATION_YAW_315             = 7,
+	ROTATION_ROLL_180            = 8,
+	ROTATION_ROLL_180_YAW_45     = 9,
+	ROTATION_ROLL_180_YAW_90     = 10,
+	ROTATION_ROLL_180_YAW_135    = 11,
+	ROTATION_PITCH_180           = 12,
+	ROTATION_ROLL_180_YAW_225    = 13,
+	ROTATION_ROLL_180_YAW_270    = 14,
+	ROTATION_ROLL_180_YAW_315    = 15,
+	ROTATION_ROLL_90             = 16,
+	ROTATION_ROLL_90_YAW_45      = 17,
+	ROTATION_ROLL_90_YAW_90      = 18,
+	ROTATION_ROLL_90_YAW_135     = 19,
+	ROTATION_ROLL_270            = 20,
+	ROTATION_ROLL_270_YAW_45     = 21,
+	ROTATION_ROLL_270_YAW_90     = 22,
+	ROTATION_ROLL_270_YAW_135    = 23,
+	ROTATION_PITCH_90            = 24,
+	ROTATION_PITCH_270           = 25,
+	ROTATION_MAX
 };
 
-typedef struct
-{
-    uint16_t roll;
-    uint16_t pitch;
-    uint16_t yaw;
+typedef struct {
+	uint16_t roll;
+	uint16_t pitch;
+	uint16_t yaw;
 } rot_lookup_t;
 
-const rot_lookup_t rot_lookup[] =
-{
-    {  0,   0,   0 },
-    {  0,   0,  45 },
-    {  0,   0,  90 },
-    {  0,   0, 135 },
-    {  0,   0, 180 },
-    {  0,   0, 225 },
-    {  0,   0, 270 },
-    {  0,   0, 315 },
-    {180,   0,   0 },
-    {180,   0,  45 },
-    {180,   0,  90 },
-    {180,   0, 135 },
-    {  0, 180,   0 },
-    {180,   0, 225 },
-    {180,   0, 270 },
-    {180,   0, 315 },
-    { 90,   0,   0 },
-    { 90,   0,  45 },
-    { 90,   0,  90 },
-    { 90,   0, 135 },
-    {270,   0,   0 },
-    {270,   0,  45 },
-    {270,   0,  90 },
-    {270,   0, 135 },
-    {  0,  90,   0 },
-    {  0, 270,   0 }
+const rot_lookup_t rot_lookup[] = {
+	{  0,   0,   0 },
+	{  0,   0,  45 },
+	{  0,   0,  90 },
+	{  0,   0, 135 },
+	{  0,   0, 180 },
+	{  0,   0, 225 },
+	{  0,   0, 270 },
+	{  0,   0, 315 },
+	{180,   0,   0 },
+	{180,   0,  45 },
+	{180,   0,  90 },
+	{180,   0, 135 },
+	{  0, 180,   0 },
+	{180,   0, 225 },
+	{180,   0, 270 },
+	{180,   0, 315 },
+	{ 90,   0,   0 },
+	{ 90,   0,  45 },
+	{ 90,   0,  90 },
+	{ 90,   0, 135 },
+	{270,   0,   0 },
+	{270,   0,  45 },
+	{270,   0,  90 },
+	{270,   0, 135 },
+	{  0,  90,   0 },
+	{  0, 270,   0 }
 };
 
 /**
@@ -239,12 +234,12 @@ public:
 private:
 	static const unsigned _rc_max_chan_count = RC_CHANNELS_MAX;	/**< maximum number of r/c channels we handle */
 
-	hrt_abstime	_ppm_last_valid;		/**< last time we got a valid ppm signal */
+	hrt_abstime	_rc_last_valid;		/**< last time we got a valid RC signal */
 
 	/**
-	 * Gather and publish PPM input data.
+	 * Gather and publish RC input data.
 	 */
-	void		ppm_poll();
+	void		rc_poll();
 
 	/* XXX should not be here - should be own driver */
 	int 		_fd_adc;			/**< ADC driver handle */
@@ -501,7 +496,7 @@ Sensors	*g_sensors = nullptr;
 }
 
 Sensors::Sensors() :
-	_ppm_last_valid(0),
+	_rc_last_valid(0),
 
 	_fd_adc(-1),
 	_last_adc(0),
@@ -532,8 +527,8 @@ Sensors::Sensors() :
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
 
-	_board_rotation(3,3),
-	_external_mag_rotation(3,3),
+	_board_rotation(3, 3),
+	_external_mag_rotation(3, 3),
 	_mag_is_external(false)
 {
 
@@ -660,9 +655,9 @@ int
 Sensors::parameters_update()
 {
 	bool rc_valid = true;
-    float tmpScaleFactor = 0.0f;
-    float tmpRevFactor = 0.0f;
-    
+	float tmpScaleFactor = 0.0f;
+	float tmpRevFactor = 0.0f;
+
 	/* rc values */
 	for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) {
 
@@ -674,19 +669,19 @@ Sensors::parameters_update()
 
 		tmpScaleFactor = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]);
 		tmpRevFactor = tmpScaleFactor * _parameters.rev[i];
-        
+
 		/* handle blowup in the scaling factor calculation */
 		if (!isfinite(tmpScaleFactor) ||
 		    (tmpRevFactor < 0.000001f) ||
-		    (tmpRevFactor > 0.2f) ) {
+		    (tmpRevFactor > 0.2f)) {
 			warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, tmpScaleFactor, (int)(_parameters.rev[i]));
 			/* scaling factors do not make sense, lock them down */
 			_parameters.scaling_factor[i] = 0.0f;
 			rc_valid = false;
+
+		} else {
+			_parameters.scaling_factor[i] = tmpScaleFactor;
 		}
-        else {
-            _parameters.scaling_factor[i] = tmpScaleFactor;
-        }
 	}
 
 	/* handle wrong values */
@@ -812,7 +807,7 @@ void
 Sensors::get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix)
 {
 	/* first set to zero */
-	rot_matrix->Matrix::zero(3,3);
+	rot_matrix->Matrix::zero(3, 3);
 
 	float roll  = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll;
 	float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch;
@@ -823,7 +818,7 @@ Sensors::get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix)
 	math::Dcm R(euler);
 
 	for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
-		(*rot_matrix)(i,j) = R(i, j);
+			(*rot_matrix)(i, j) = R(i, j);
 }
 
 void
@@ -841,7 +836,7 @@ Sensors::accel_init()
 
 		// XXX do the check more elegantly
 
-		#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 
 		/* set the accel internal sampling rate up to at leat 1000Hz */
 		ioctl(fd, ACCELIOCSSAMPLERATE, 1000);
@@ -849,7 +844,7 @@ Sensors::accel_init()
 		/* set the driver to poll at 1000Hz */
 		ioctl(fd, SENSORIOCSPOLLRATE, 1000);
 
-		#elif CONFIG_ARCH_BOARD_PX4FMU_V2
+#elif CONFIG_ARCH_BOARD_PX4FMU_V2
 
 		/* set the accel internal sampling rate up to at leat 800Hz */
 		ioctl(fd, ACCELIOCSSAMPLERATE, 800);
@@ -857,10 +852,10 @@ Sensors::accel_init()
 		/* set the driver to poll at 800Hz */
 		ioctl(fd, SENSORIOCSPOLLRATE, 800);
 
-		#else
-			#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
+#else
+#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
 
-		#endif
+#endif
 
 		warnx("using system accel");
 		close(fd);
@@ -882,7 +877,7 @@ Sensors::gyro_init()
 
 		// XXX do the check more elegantly
 
-		#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 
 		/* set the gyro internal sampling rate up to at least 1000Hz */
 		if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK)
@@ -892,7 +887,7 @@ Sensors::gyro_init()
 		if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK)
 			ioctl(fd, SENSORIOCSPOLLRATE, 800);
 
-		#else
+#else
 
 		/* set the gyro internal sampling rate up to at least 760Hz */
 		ioctl(fd, GYROIOCSSAMPLERATE, 760);
@@ -900,7 +895,7 @@ Sensors::gyro_init()
 		/* set the driver to poll at 760Hz */
 		ioctl(fd, SENSORIOCSPOLLRATE, 760);
 
-		#endif
+#endif
 
 		warnx("using system gyro");
 		close(fd);
@@ -924,23 +919,28 @@ Sensors::mag_init()
 
 
 	ret = ioctl(fd, MAGIOCSSAMPLERATE, 150);
+
 	if (ret == OK) {
 		/* set the pollrate accordingly */
 		ioctl(fd, SENSORIOCSPOLLRATE, 150);
+
 	} else {
 		ret = ioctl(fd, MAGIOCSSAMPLERATE, 100);
+
 		/* if the slower sampling rate still fails, something is wrong */
 		if (ret == OK) {
 			/* set the driver to poll also at the slower rate */
 			ioctl(fd, SENSORIOCSPOLLRATE, 100);
+
 		} else {
 			errx(1, "FATAL: mag sampling rate could not be set");
 		}
 	}
 
-	
+
 
 	ret = ioctl(fd, MAGIOCGEXTERNAL, 0);
+
 	if (ret < 0)
 		errx(1, "FATAL: unknown if magnetometer is external or onboard");
 	else if (ret == 1)
@@ -993,7 +993,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 		orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
 
 		math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z};
-		vect = _board_rotation*vect;
+		vect = _board_rotation * vect;
 
 		raw.accelerometer_m_s2[0] = vect(0);
 		raw.accelerometer_m_s2[1] = vect(1);
@@ -1019,7 +1019,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 		orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
 
 		math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z};
-		vect = _board_rotation*vect;
+		vect = _board_rotation * vect;
 
 		raw.gyro_rad_s[0] = vect(0);
 		raw.gyro_rad_s[1] = vect(1);
@@ -1047,9 +1047,9 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 		math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z};
 
 		if (_mag_is_external)
-			vect = _external_mag_rotation*vect;
+			vect = _external_mag_rotation * vect;
 		else
-			vect = _board_rotation*vect;
+			vect = _board_rotation * vect;
 
 		raw.magnetometer_ga[0] = vect(0);
 		raw.magnetometer_ga[1] = vect(1);
@@ -1094,8 +1094,8 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
 		raw.differential_pressure_counter++;
 
 		_airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_pa);
-		_airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar*1e2f, 
-											 			 raw.baro_pres_mbar*1e2f, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
+		_airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar * 1e2f,
+					      raw.baro_pres_mbar * 1e2f, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
 
 		/* announce the airspeed if needed, just publish else */
 		if (_airspeed_pub > 0) {
@@ -1236,12 +1236,12 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 		int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc));
 
 		for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) {
-			
+
 			if (ret >= (int)sizeof(buf_adc[0])) {
 
 				/* Save raw voltage values */
 				if (i < (sizeof(raw.adc_voltage_v)) / sizeof(raw.adc_voltage_v[0])) {
-					 raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f);
+					raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f);
 				}
 
 				/* look for specific channels and process the raw voltage to measurement data */
@@ -1269,12 +1269,12 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 						} else {
 							_battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status);
 						}
-					} 
+					}
 
 				} else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
 
 					/* calculate airspeed, raw is the difference from */
-					float voltage = (float)(buf_adc[i].am_data ) * 3.3f / 4096.0f * 2.0f; //V_ref/4096 * (voltage divider factor)
+					float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f * 2.0f;  //V_ref/4096 * (voltage divider factor)
 
 					/**
 					 * The voltage divider pulls the signal down, only act on
@@ -1306,17 +1306,13 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 }
 
 void
-Sensors::ppm_poll()
+Sensors::rc_poll()
 {
+	bool rc_updated;
+	orb_check(_rc_sub, &rc_updated);
 
-	/* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */
-	struct pollfd fds[1];
-	fds[0].fd = _rc_sub;
-	fds[0].events = POLLIN;
-	/* check non-blocking for new data */
-	int poll_ret = poll(fds, 1, 0);
-
-	if (poll_ret > 0) {
+	if (rc_updated) {
+		/* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */
 		struct rc_input_values	rc_input;
 
 		orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
@@ -1352,7 +1348,7 @@ Sensors::ppm_poll()
 			channel_limit = _rc_max_chan_count;
 
 		/* we are accepting this message */
-		_ppm_last_valid = rc_input.timestamp;
+		_rc_last_valid = rc_input.timestamp;
 
 		/* Read out values from raw message */
 		for (unsigned int i = 0; i < channel_limit; i++) {
@@ -1362,6 +1358,7 @@ Sensors::ppm_poll()
 			 */
 			if (rc_input.values[i] < _parameters.min[i])
 				rc_input.values[i] = _parameters.min[i];
+
 			if (rc_input.values[i] > _parameters.max[i])
 				rc_input.values[i] = _parameters.max[i];
 
@@ -1622,7 +1619,7 @@ Sensors::task_main()
 			orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw);
 
 		/* Look for new r/c input data */
-		ppm_poll();
+		rc_poll();
 
 		perf_end(_loop_perf);
 	}
@@ -1640,11 +1637,11 @@ Sensors::start()
 
 	/* start the task */
 	_sensors_task = task_spawn_cmd("sensors_task",
-				   SCHED_DEFAULT,
-				   SCHED_PRIORITY_MAX - 5,
-				   2048,
-				   (main_t)&Sensors::task_main_trampoline,
-				   nullptr);
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       2048,
+				       (main_t)&Sensors::task_main_trampoline,
+				       nullptr);
 
 	if (_sensors_task < 0) {
 		warn("task start failed");

src/modules/systemlib/mixer/mixer.cpp

@@ -50,6 +50,8 @@
 #include <stdio.h>
 #include <math.h>
 #include <unistd.h>
+#include <ctype.h>
+#include <systemlib/err.h>
 
 #include "mixer.h"
 
@@ -114,6 +116,33 @@ Mixer::scale_check(struct mixer_scaler_s &scaler)
 	return 0;
 }
 
+const char *
+Mixer::findtag(const char *buf, unsigned &buflen, char tag)
+{
+	while (buflen >= 2) {
+		if ((buf[0] == tag) && (buf[1] == ':'))
+			return buf;
+		buf++;
+		buflen--;
+	}
+	return nullptr;
+}
+
+const char *
+Mixer::skipline(const char *buf, unsigned &buflen)
+{
+	const char *p;
+
+	/* if we can find a CR or NL in the buffer, skip up to it */
+	if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen))) {
+		/* skip up to it AND one beyond - could be on the NUL symbol now */
+		buflen -= (p - buf) + 1;
+		return p + 1;
+	}
+
+	return nullptr;
+}
+
 /****************************************************************************/
 
 NullMixer::NullMixer() :

src/modules/systemlib/mixer/mixer.h

@@ -128,7 +128,9 @@
 #ifndef _SYSTEMLIB_MIXER_MIXER_H
 #define _SYSTEMLIB_MIXER_MIXER_H value
 
+#include <nuttx/config.h>
 #include "drivers/drv_mixer.h"
+#include "mixer_load.h"
 
 /**
  * Abstract class defining a mixer mixing zero or more inputs to
@@ -210,6 +212,24 @@ protected:
 	 */
 	static int			scale_check(struct mixer_scaler_s &scaler);
 
+	/**
+	 * Find a tag
+	 *
+	 * @param buf			The buffer to operate on.
+	 * @param buflen		length of the buffer.
+	 * @param tag			character to search for.
+	 */
+	static const char *		findtag(const char *buf, unsigned &buflen, char tag);
+
+	/**
+	 * Skip a line
+	 *
+	 * @param buf			The buffer to operate on.
+	 * @param buflen		length of the buffer.
+	 * @return			0 / OK if a line could be skipped, 1 else
+	 */
+	static const char *		skipline(const char *buf, unsigned &buflen);
+
 private:
 };
 
@@ -238,6 +258,11 @@ public:
 	 */
 	void				reset();
 
+	/**
+	 * Count the mixers in the group.
+	 */
+	unsigned			count();
+
 	/**
 	 * Adds mixers to the group based on a text description in a buffer.
 	 *

src/modules/systemlib/mixer/mixer_group.cpp

@@ -111,6 +111,20 @@ MixerGroup::mix(float *outputs, unsigned space)
 	return index;
 }
 
+unsigned
+MixerGroup::count()
+{
+	Mixer	*mixer = _first;
+	unsigned index = 0;
+
+	while ((mixer != nullptr)) {
+		mixer = mixer->_next;
+		index++;
+	}
+
+	return index;
+}
+
 void
 MixerGroup::groups_required(uint32_t &groups)
 {
@@ -170,6 +184,7 @@ MixerGroup::load_from_buf(const char *buf, unsigned &buflen)
 
 			/* only adjust buflen if parsing was successful */
 			buflen = resid;
+			debug("SUCCESS - buflen: %d", buflen);
 		} else {
 
 			/*

src/modules/systemlib/mixer/mixer_load.c

@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mixer_load.c
+ *
+ * Programmable multi-channel mixer library.
+ */
+
+#include <nuttx/config.h>
+#include <string.h>
+#include <stdio.h>
+#include <ctype.h>
+
+#include "mixer_load.h"
+
+int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
+{
+	FILE		*fp;
+	char		line[120];
+
+	/* open the mixer definition file */
+	fp = fopen(fname, "r");
+	if (fp == NULL) {
+		return 1;
+	}
+
+	/* read valid lines from the file into a buffer */
+	buf[0] = '\0';
+	for (;;) {
+
+		/* get a line, bail on error/EOF */
+		line[0] = '\0';
+		if (fgets(line, sizeof(line), fp) == NULL)
+			break;
+
+		/* if the line doesn't look like a mixer definition line, skip it */
+		if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':'))
+			continue;
+
+		/* compact whitespace in the buffer */
+		char *t, *f;
+		for (f = line; *f != '\0'; f++) {
+			/* scan for space characters */
+			if (*f == ' ') {
+				/* look for additional spaces */
+				t = f + 1;
+				while (*t == ' ')
+					t++;
+				if (*t == '\0') {
+					/* strip trailing whitespace */
+					*f = '\0';
+				} else if (t > (f + 1)) {
+					memmove(f + 1, t, strlen(t) + 1);
+				}
+			}
+		}
+
+		/* if the line is too long to fit in the buffer, bail */
+		if ((strlen(line) + strlen(buf) + 1) >= maxlen) {
+			return 1;
+		}
+
+		/* add the line to the buffer */
+		strcat(buf, line);
+	}
+
+	return 0;
+}
+

src/modules/systemlib/mixer/mixer_load.h

@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mixer_load.h
+ *
+ */
+
+
+#ifndef _SYSTEMLIB_MIXER_LOAD_H
+#define _SYSTEMLIB_MIXER_LOAD_H value
+
+#include <nuttx/config.h>
+
+__BEGIN_DECLS
+
+__EXPORT int load_mixer_file(const char *fname, char *buf, unsigned maxlen);
+
+__END_DECLS
+
+#endif

src/modules/systemlib/mixer/mixer_multirotor.cpp

@@ -130,7 +130,7 @@ const MultirotorMixer::Rotor _config_octa_plus[] = {
 	{  1.000000,  0.000000, -1.00 },
 	{ -1.000000,  0.000000, -1.00 },
 };
-const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOMETRY] = {
+const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = {
 	&_config_quad_x[0],
 	&_config_quad_plus[0],
 	&_config_quad_v[0],
@@ -140,7 +140,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOME
 	&_config_octa_x[0],
 	&_config_octa_plus[0],
 };
-const unsigned _config_rotor_count[MultirotorMixer::Geometry::MAX_GEOMETRY] = {
+const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = {
 	4, /* quad_x */
 	4, /* quad_plus */
 	4, /* quad_v */
@@ -205,11 +205,17 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
 	}
 
 	if (used > (int)buflen) {
-		debug("multirotor spec used %d of %u", used, buflen);
+		debug("OVERFLOW: multirotor spec used %d of %u", used, buflen);
 		return nullptr;
 	}
 
-	buflen -= used;
+	buf = skipline(buf, buflen);
+	if (buf == nullptr) {
+		debug("no line ending, line is incomplete");
+		return nullptr;
+	}
+
+	debug("remaining in buf: %d, first char: %c", buflen, buf[0]);
 
 	if (!strcmp(geomname, "4+")) {
 		geometry = MultirotorMixer::QUAD_PLUS;

src/modules/systemlib/mixer/mixer_simple.cpp

@@ -55,7 +55,7 @@
 #include "mixer.h"
 
 #define debug(fmt, args...)	do { } while(0)
-// #define debug(fmt, args...)	do { printf("[mixer] " fmt "\n", ##args); } while(0)
+//#define debug(fmt, args...)	do { printf("[mixer] " fmt "\n", ##args); } while(0)
 
 SimpleMixer::SimpleMixer(ControlCallback control_cb,
 			 uintptr_t cb_handle,
@@ -71,28 +71,6 @@ SimpleMixer::~SimpleMixer()
 		free(_info);
 }
 
-static const char *
-findtag(const char *buf, unsigned &buflen, char tag)
-{
-	while (buflen >= 2) {
-		if ((buf[0] == tag) && (buf[1] == ':'))
-			return buf;
-		buf++;
-		buflen--;
-	}
-	return nullptr;
-}
-
-static void
-skipline(const char *buf, unsigned &buflen)
-{
-	const char *p;
-
-	/* if we can find a CR or NL in the buffer, skip up to it */
-	if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen)))
-		buflen -= (p - buf);
-}
-
 int
 SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler)
 {
@@ -111,7 +89,12 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler
 		debug("out scaler parse failed on '%s' (got %d, consumed %d)", buf, ret, n);
 		return -1;
 	}
-	skipline(buf, buflen);
+
+	buf = skipline(buf, buflen);
+	if (buf == nullptr) {
+		debug("no line ending, line is incomplete");
+		return -1;
+	}
 
 	scaler.negative_scale	= s[0] / 10000.0f;
 	scaler.positive_scale	= s[1] / 10000.0f;
@@ -130,7 +113,7 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale
 
 	buf = findtag(buf, buflen, 'S');
 	if ((buf == nullptr) || (buflen < 16)) {
-		debug("contorl parser failed finding tag, ret: '%s'", buf);
+		debug("control parser failed finding tag, ret: '%s'", buf);
 		return -1;
 	}
 
@@ -139,7 +122,12 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale
 		debug("control parse failed on '%s'", buf);
 		return -1;
 	}
-	skipline(buf, buflen);
+
+	buf = skipline(buf, buflen);
+	if (buf == nullptr) {
+		debug("no line ending, line is incomplete");
+		return -1;
+	}
 
 	control_group		= u[0];
 	control_index		= u[1];
@@ -161,29 +149,17 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c
 	int used;
 	const char *end = buf + buflen;
 
-	/* enforce that the mixer ends with space or a new line */
-	for (int i = buflen - 1; i >= 0; i--) {
-		if (buf[i] == '\0')
-			continue;
-
-		/* require a space or newline at the end of the buffer, fail on printable chars */
-		if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') {
-			/* found a line ending or space, so no split symbols / numbers. good. */
-			break;
-		} else {
-			debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen-1, buf[i]);
-			goto out;
-		}
-
-	}
-
 	/* get the base info for the mixer */
 	if (sscanf(buf, "M: %u%n", &inputs, &used) != 1) {
 		debug("simple parse failed on '%s'", buf);
 		goto out;
 	}
 
-	buflen -= used;
+	buf = skipline(buf, buflen);
+	if (buf == nullptr) {
+		debug("no line ending, line is incomplete");
+		goto out;
+	}
 
 	mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs));
 

src/modules/systemlib/mixer/module.mk

@@ -39,4 +39,5 @@ LIBNAME	= mixerlib
 SRCS		= mixer.cpp \
 		  mixer_group.cpp \
 		  mixer_multirotor.cpp \
-		  mixer_simple.cpp
+		  mixer_simple.cpp \
+		  mixer_load.c

src/modules/uORB/topics/vehicle_control_mode.h

@@ -61,7 +61,6 @@
  */
 struct vehicle_control_mode_s
 {
-	uint16_t counter;   /**< incremented by the writing thread every time new data is stored */
 	uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
 
 	bool flag_armed;

src/systemcmds/mixer/mixer.cpp

@@ -47,10 +47,15 @@
 #include <nuttx/compiler.h>
 
 #include <systemlib/err.h>
-#include <drivers/drv_mixer.h>
+#include <systemlib/mixer/mixer.h>
 #include <uORB/topics/actuator_controls.h>
 
-__EXPORT int mixer_main(int argc, char *argv[]);
+/**
+ * Mixer utility for loading mixer files to devices
+ *
+ * @ingroup apps
+ */
+extern "C" __EXPORT int mixer_main(int argc, char *argv[]);
 
 static void	usage(const char *reason) noreturn_function;
 static void	load(const char *devname, const char *fname) noreturn_function;
@@ -87,8 +92,6 @@ static void
 load(const char *devname, const char *fname)
 {
 	int		dev;
-	FILE		*fp;
-	char		line[120];
 	char		buf[2048];
 
 	/* open the device */
@@ -99,49 +102,7 @@ load(const char *devname, const char *fname)
 	if (ioctl(dev, MIXERIOCRESET, 0))
 		err(1, "can't reset mixers on %s", devname);
 
-	/* open the mixer definition file */
-	fp = fopen(fname, "r");
-	if (fp == NULL)
-		err(1, "can't open %s", fname);
-
-	/* read valid lines from the file into a buffer */
-	buf[0] = '\0';
-	for (;;) {
-
-		/* get a line, bail on error/EOF */
-		line[0] = '\0';
-		if (fgets(line, sizeof(line), fp) == NULL)
-			break;
-
-		/* if the line doesn't look like a mixer definition line, skip it */
-		if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':'))
-			continue;
-
-		/* compact whitespace in the buffer */
-		char *t, *f;
-		for (f = buf; *f != '\0'; f++) {
-			/* scan for space characters */
-			if (*f == ' ') {
-				/* look for additional spaces */
-				t = f + 1;
-				while (*t == ' ')
-					t++;
-				if (*t == '\0') {
-					/* strip trailing whitespace */
-					*f = '\0';
-				} else if (t > (f + 1)) {
-					memmove(f + 1, t, strlen(t) + 1);
-				}
-			}
-		}
-
-		/* if the line is too long to fit in the buffer, bail */
-		if ((strlen(line) + strlen(buf) + 1) >= sizeof(buf))
-			break;
-
-		/* add the line to the buffer */
-		strcat(buf, line);
-	}
+	load_mixer_file(fname, &buf[0], sizeof(buf));
 
 	/* XXX pass the buffer to the device */
 	int ret = ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf);

src/systemcmds/mixer/module.mk

@@ -36,7 +36,7 @@
 #
 
 MODULE_COMMAND	 = mixer
-SRCS		 = mixer.c
+SRCS		 = mixer.cpp
 
 MODULE_STACKSIZE = 4096
 

src/systemcmds/tests/module.mk

@@ -23,6 +23,7 @@ SRCS			 = test_adc.c \
 			   test_uart_console.c \
 			   test_uart_loopback.c \
 			   test_uart_send.c \
+			   test_mixer.cpp \
 			   tests_file.c \
 			   tests_main.c \
 			   tests_param.c

src/systemcmds/tests/test_mixer.cpp

@@ -0,0 +1,199 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file test_mixer.hpp
+ *
+ * Mixer load test
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <string.h>
+#include <time.h>
+
+#include <systemlib/err.h>
+#include <systemlib/mixer/mixer.h>
+
+#include "tests.h"
+
+static int	mixer_callback(uintptr_t handle,
+			       uint8_t control_group,
+			       uint8_t control_index,
+			       float &control);
+
+int test_mixer(int argc, char *argv[])
+{
+	warnx("testing mixer");
+
+	char *filename = "/etc/mixers/IO_pass.mix";
+
+	if (argc > 1)
+		filename = argv[1];
+
+	warnx("loading: %s", filename);
+
+	char		buf[2048];
+
+	load_mixer_file(filename, &buf[0], sizeof(buf));
+	unsigned loaded = strlen(buf);
+
+	warnx("loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded);
+
+	/* load the mixer in chunks, like
+	 * in the case of a remote load,
+	 * e.g. on PX4IO.
+	 */
+
+	unsigned nused = 0;
+
+	const unsigned chunk_size = 64;
+
+	MixerGroup mixer_group(mixer_callback, 0);
+
+	/* load at once test */
+	unsigned xx = loaded;
+	mixer_group.load_from_buf(&buf[0], xx);
+	warnx("complete buffer load: loaded %u mixers", mixer_group.count());
+	if (mixer_group.count() != 8)
+		return 1;
+
+	unsigned empty_load = 2;
+	char empty_buf[2];
+	empty_buf[0] = ' ';
+	empty_buf[1] = '\0';
+	mixer_group.reset();
+	mixer_group.load_from_buf(&empty_buf[0], empty_load);
+	warnx("empty buffer load: loaded %u mixers, used: %u", mixer_group.count(), empty_load);
+	if (empty_load != 0)
+		return 1;
+
+	/* FIRST mark the mixer as invalid */
+	bool mixer_ok = false;
+	/* THEN actually delete it */
+	mixer_group.reset();
+	char mixer_text[256];		/* large enough for one mixer */
+	unsigned mixer_text_length = 0;
+
+	unsigned transmitted = 0;
+
+	warnx("transmitted: %d, loaded: %d", transmitted, loaded);
+
+	while (transmitted < loaded) {
+
+		unsigned	text_length = (loaded - transmitted > chunk_size) ? chunk_size : loaded - transmitted;
+
+		/* check for overflow - this would be really fatal */
+		if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
+			bool mixer_ok = false;
+			return 1;
+		}
+
+		/* append mixer text and nul-terminate */
+		memcpy(&mixer_text[mixer_text_length], &buf[transmitted], text_length);
+		mixer_text_length += text_length;
+		mixer_text[mixer_text_length] = '\0';
+		warnx("buflen %u, text:\n\"%s\"", mixer_text_length, &mixer_text[0]);
+
+		/* process the text buffer, adding new mixers as their descriptions can be parsed */
+		unsigned resid = mixer_text_length;
+		mixer_group.load_from_buf(&mixer_text[0], resid);
+
+		/* if anything was parsed */
+		if (resid != mixer_text_length) {
+
+			/* only set mixer ok if no residual is left over */
+			if (resid == 0) {
+				mixer_ok = true;
+			} else {
+				/* not yet reached the end of the mixer, set as not ok */
+				mixer_ok = false;
+			}
+
+			warnx("used %u", mixer_text_length - resid);
+
+			/* copy any leftover text to the base of the buffer for re-use */
+			if (resid > 0)
+				memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
+
+			mixer_text_length = resid;
+		}
+
+		transmitted += text_length;
+	}
+
+	warnx("chunked load: loaded %u mixers", mixer_group.count());
+
+	if (mixer_group.count() != 8)
+		return 1;
+
+	/* load multirotor at once test */
+	mixer_group.reset();
+
+	if (argc > 2)
+		filename = argv[2];
+	else
+		filename = "/etc/mixers/FMU_quad_w.mix";
+
+	load_mixer_file(filename, &buf[0], sizeof(buf));
+	loaded = strlen(buf);
+
+	warnx("loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded);
+
+	unsigned mc_loaded = loaded;
+	mixer_group.load_from_buf(&buf[0], mc_loaded);
+	warnx("complete buffer load: loaded %u mixers", mixer_group.count());
+	if (mixer_group.count() != 8)
+		return 1;
+}
+
+static int
+mixer_callback(uintptr_t handle,
+	       uint8_t control_group,
+	       uint8_t control_index,
+	       float &control)
+{
+	if (control_group != 0)
+		return -1;
+
+	control = 0.0f;
+
+	return 0;
+}

src/systemcmds/tests/tests.h

@@ -76,6 +76,8 @@
  * Public Function Prototypes
  ****************************************************************************/
 
+__BEGIN_DECLS
+
 extern int	test_sensors(int argc, char *argv[]);
 extern int	test_gpio(int argc, char *argv[]);
 extern int	test_hrt(int argc, char *argv[]);
@@ -98,5 +100,8 @@ extern int	test_jig_voltages(int argc, char *argv[]);
 extern int	test_param(int argc, char *argv[]);
 extern int	test_bson(int argc, char *argv[]);
 extern int	test_file(int argc, char *argv[]);
+extern int	test_mixer(int argc, char *argv[]);
+
+__END_DECLS
 
 #endif /* __APPS_PX4_TESTS_H */

src/systemcmds/tests/tests_main.c

@@ -110,6 +110,7 @@ const struct {
 	{"param",		test_param,	0},
 	{"bson",		test_bson,	0},
 	{"file",		test_file,	0},
+	{"mixer",		test_mixer,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"help",		test_help,	OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
 	{NULL,			NULL, 		0}
 };