Merge branch 'master' into gpio_led_fmuv2

.gitignore

@@ -35,3 +35,4 @@ mavlink/include/mavlink/v0.9/
 /Documentation/doxygen*objdb*tmp
 .tags
 .tags_sorted_by_file
+.pydevproject

ROMFS/px4fmu_common/init.d/10015_tbs_discovery

@@ -2,7 +2,7 @@
 #
 # Team Blacksheep Discovery Quadcopter
 #
-# Anton Babushkin <anton.babushkin@me.com>, Simon Wilks <sjwilks@gmail.com>
+# Anton Babushkin <anton.babushkin@me.com>, Simon Wilks <sjwilks@gmail.com>, Thomas Gubler <thomasgubler@gmail.com>
 #
 
 sh /etc/init.d/rc.mc_defaults
@@ -15,7 +15,7 @@ then
 	param set MC_ROLLRATE_I 0.05
 	param set MC_ROLLRATE_D 0.0017
 	param set MC_PITCH_P 8.0
-	param set MC_PITCHRATE_P 0.14
+	param set MC_PITCHRATE_P 0.1
 	param set MC_PITCHRATE_I 0.1
 	param set MC_PITCHRATE_D 0.0025
 	param set MC_YAW_P 2.8

ROMFS/px4fmu_common/init.d/3031_phantom

@@ -5,4 +5,6 @@
 # Simon Wilks <sjwilks@gmail.com>
 #
 
+sh /etc/init.d/rc.fw_defaults
+
 set MIXER FMU_Q

ROMFS/px4fmu_common/init.d/4008_ardrone

@@ -0,0 +1,35 @@
+#!nsh
+#
+# ARDrone
+#
+
+echo "[init] 4008_ardrone: PX4FMU on PX4IOAR carrier board"
+
+# Just use the default multicopter settings.
+sh /etc/init.d/rc.mc_defaults
+
+#
+# Load default params for this platform
+#
+if [ $DO_AUTOCONFIG == yes ]
+then
+    # Set all params here, then disable autoconfig
+    param set MC_ROLL_P 5.0
+    param set MC_ROLLRATE_P 0.13
+    param set MC_ROLLRATE_I 0.0
+    param set MC_ROLLRATE_D 0.0
+    param set MC_PITCH_P 5.0
+    param set MC_PITCHRATE_P 0.13
+    param set MC_PITCHRATE_I 0.0
+    param set MC_PITCHRATE_D 0.0
+    param set MC_YAW_P 1.0
+    param set MC_YAW_D 0.1
+    param set MC_YAWRATE_P 0.15
+    param set MC_YAWRATE_I 0.0
+    param set MC_YAWRATE_D 0.0
+    param set MC_YAW_FF 0.15
+fi
+
+set OUTPUT_MODE ardrone
+set USE_IO no
+set MIXER skip

ROMFS/px4fmu_common/init.d/rc.autostart

@@ -106,6 +106,15 @@ then
 	sh /etc/init.d/4001_quad_x
 fi
 
+#
+# ARDrone
+#
+
+if param compare SYS_AUTOSTART 4008 8
+then
+    sh /etc/init.d/4008_ardrone
+fi
+
 if param compare SYS_AUTOSTART 4010 10
 then
 	sh /etc/init.d/4010_dji_f330

ROMFS/px4fmu_common/init.d/rc.fw_defaults

@@ -10,4 +10,5 @@ then
 	param set NAV_LAND_ALT 90
 	param set NAV_RTL_ALT 100
 	param set NAV_RTL_LAND_T -1
+	param set NAV_ACCEPT_RAD 50
 fi

ROMFS/px4fmu_common/init.d/rc.interface

@@ -3,7 +3,7 @@
 # Script to configure control interface
 #
 
-if [ $MIXER != none ]
+if [ $MIXER != none -a $MIXER != skip ]
 then
 	#
 	# Load mixer
@@ -33,8 +33,11 @@ then
 		tone_alarm $TUNE_OUT_ERROR
 	fi
 else
-	echo "[init] Mixer not defined"
-	tone_alarm $TUNE_OUT_ERROR
+	if [ $MIXER != skip ]
+	then
+		echo "[init] Mixer not defined"
+		tone_alarm $TUNE_OUT_ERROR
+	fi
 fi
 
 if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ]

ROMFS/px4fmu_common/init.d/rcS

@@ -240,6 +240,11 @@ then
 		fi
 	fi
 
+	if [ $OUTPUT_MODE == ardrone ]
+	then
+		set FMU_MODE gpio_serial
+	fi
+
 	if [ $HIL == yes ]
 	then
 		set OUTPUT_MODE hil
@@ -277,9 +282,9 @@ then
 				tone_alarm $TUNE_OUT_ERROR
 			fi
 		fi
-		if [ $OUTPUT_MODE == fmu ]
+		if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == ardrone ]
 		then
-			echo "[init] Use FMU PWM as primary output"
+			echo "[init] Use FMU as primary output"
 			if fmu mode_$FMU_MODE
 			then
 				echo "[init] FMU mode_$FMU_MODE started"
@@ -294,7 +299,7 @@ then
 				then
 					set TTYS1_BUSY yes
 				fi
-				if [ $FMU_MODE == pwm_gpio ]
+				if [ $FMU_MODE == pwm_gpio -o $OUTPUT_MODE == ardrone ]
 				then
 					set TTYS1_BUSY yes
 				fi
@@ -351,7 +356,7 @@ then
 				fi
 			fi
 		else
-			if [ $OUTPUT_MODE != fmu ]
+			if [ $OUTPUT_MODE != fmu -a $OUTPUT_MODE != ardrone ]
 			then
 				if fmu mode_$FMU_MODE
 				then
@@ -367,7 +372,7 @@ then
 					then
 						set TTYS1_BUSY yes
 					fi
-					if [ $FMU_MODE == pwm_gpio ]
+					if [ $FMU_MODE == pwm_gpio -o $OUTPUT_MODE == ardrone ]
 					then
 						set TTYS1_BUSY yes
 					fi
@@ -427,6 +432,14 @@ then
 		gps start
 	fi
 	
+	#
+	# Start up ARDrone Motor interface
+	#
+	if [ $OUTPUT_MODE == ardrone ]
+	then
+		ardrone_interface start -d /dev/ttyS1
+	fi
+
 	#
 	# Fixed wing setup
 	#

ROMFS/px4fmu_logging/init.d/rcS

@@ -1,88 +0,0 @@
-#!nsh
-#
-# PX4FMU startup script for logging purposes
-#
-
-#
-# Try to mount the microSD card.
-#
-echo "[init] looking for microSD..."
-if mount -t vfat /dev/mmcsd0 /fs/microsd
-then
-	echo "[init] card mounted at /fs/microsd"
-	# Start playing the startup tune
-	tone_alarm start
-else
-	echo "[init] no microSD card found"
-	# Play SOS
-	tone_alarm error
-fi
-
-uorb start
-
-#
-# Start sensor drivers here.
-#
-
-ms5611 start
-adc start
-
-# mag might be external
-if hmc5883 start
-then
-	echo "using HMC5883"
-fi
-
-if mpu6000 start
-then
-	echo "using MPU6000"
-fi
-
-if l3gd20 start
-then
-	echo "using L3GD20(H)"
-fi
-
-if lsm303d start
-then
-	set BOARD fmuv2
-else
-	set BOARD fmuv1
-fi
-
-# Start airspeed sensors
-if meas_airspeed start
-then
-	echo "using MEAS airspeed sensor"
-else
-	if ets_airspeed start
-	then
-		echo "using ETS airspeed sensor (bus 3)"
-	else
-		if ets_airspeed start -b 1
-		then
-			echo "Using ETS airspeed sensor (bus 1)"
-		fi
-	fi
-fi
-
-#
-# Start the sensor collection task.
-# IMPORTANT: this also loads param offsets
-# ALWAYS start this task before the
-# preflight_check.
-#
-if sensors start
-then
-	echo "SENSORS STARTED"
-fi
-
-sdlog2 start -r 250 -e -b 16
-
-if sercon
-then
-	echo "[init] USB interface connected"
-
-	# Try to get an USB console
-	nshterm /dev/ttyACM0 &
-fi

Tools/fix_code_style.sh

@@ -16,4 +16,5 @@ astyle \
     --ignore-exclude-errors-x	\
     --lineend=linux		\
     --exclude=EASTL		\
+    --add-brackets \
     $*

Tools/fix_code_style_ubuntu.sh

@@ -1,19 +0,0 @@
-#!/bin/sh
-astyle \
-    --style=linux		\
-    --indent=force-tab=8	\
-    --indent-cases		\
-    --indent-preprocessor	\
-    --break-blocks=all		\
-    --pad-oper			\
-    --pad-header		\
-    --unpad-paren		\
-    --keep-one-line-blocks	\
-    --keep-one-line-statements	\
-    --align-pointer=name	\
-    --suffix=none		\
-    --lineend=linux		\
-    $*
-    #--ignore-exclude-errors-x	\
-    #--exclude=EASTL		\
-    #--align-reference=name	\

Tools/px4params/.gitignore

@@ -1,3 +1,4 @@
 parameters.wiki
 parameters.xml
+parameters.wikirpc.xml
 cookies.txt

Tools/px4params/dokuwikiout.py

@@ -1,62 +0,0 @@
-import output
-from xml.sax.saxutils import escape
-
-class DokuWikiOutput(output.Output):
-    def Generate(self, groups):
-        pre_text = """<?xml version='1.0'?>
-            <methodCall>
-              <methodName>wiki.putPage</methodName>
-                  <params>
-              <param> 
-                <value>
-                  <string>:firmware:parameters</string>
-                </value>
-              </param> 
-                  <param> 
-                <value>
-                  <string>"""
-        result = "====== Parameter Reference ======\nThis list is auto-generated every few minutes and contains the most recent parameter names and default values."
-        for group in groups:
-            result += "==== %s ====\n\n" % group.GetName()
-            result += "|< 100% 20% 20% 10% 10% 10% 30%>|\n"
-            result += "^ Name            ^ Description    ^ Min    ^ Max    ^ Default   ^ Comment    ^\n"
-            for param in group.GetParams():
-                code = param.GetFieldValue("code")
-                name = param.GetFieldValue("short_desc")
-                name = name.replace("\n", "")
-                result += "| %s   | %s  " % (code, name)
-                min_val = param.GetFieldValue("min")
-                if min_val is not None:
-                    result += " | %s  " % min_val
-                else:
-                    result += " | "
-                max_val = param.GetFieldValue("max")
-                if max_val is not None:
-                    result += " | %s  " % max_val
-                else:
-                    result += " | "
-                def_val = param.GetFieldValue("default")
-                if def_val is not None:
-                    result += "| %s  " % def_val
-                else:
-                    result += " | "
-                long_desc = param.GetFieldValue("long_desc")
-                if long_desc is not None:
-                    long_desc = long_desc.replace("\n", "")
-                    result += "| %s  " % long_desc
-                else:
-                    result += " | "
-                result += " |\n"
-            result += "\n"
-        post_text = """</string>
-        </value>
-      </param> 
-           <param> 
-        <value>
-            <name>sum</name>
-            <string>Updated parameters automagically from code.</string>
-        </value>
-      </param> 
-    </params> 
-    </methodCall>"""
-        return pre_text + escape(result) + post_text

Tools/px4params/output.py

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

Tools/px4params/output_dokuwiki_listings.py

@@ -1,7 +1,7 @@
-import output
+import codecs
 
-class DokuWikiOutput(output.Output):
-    def Generate(self, groups):
+class DokuWikiListingsOutput():
+    def __init__(self, groups):
         result = ""
         for group in groups:
             result += "==== %s ====\n\n" % group.GetName()
@@ -24,4 +24,8 @@ class DokuWikiOutput(output.Output):
                 if def_val is not None:
                     result += "* Default value: %s\n" % def_val
                 result += "\n"
-        return result
+        self.output = result
+
+    def Save(self, filename):
+        with codecs.open(filename, 'w', 'utf-8') as f:
+            f.write(self.output)

Tools/px4params/output_dokuwiki_tables.py

@@ -0,0 +1,76 @@
+from xml.sax.saxutils import escape
+import codecs
+
+class DokuWikiTablesOutput():
+    def __init__(self, groups):
+        result = "====== Parameter Reference ======\nThis list is auto-generated every few minutes and contains the most recent parameter names and default values.\n\n"
+        for group in groups:
+            result += "==== %s ====\n\n" % group.GetName()
+            result += "|< 100% 20% 20% 10% 10% 10% 30%>|\n"
+            result += "^ Name ^ Description ^  Min ^  Max ^  Default ^ Comment ^\n"
+            for param in group.GetParams():
+                code = param.GetFieldValue("code")
+                name = param.GetFieldValue("short_desc")
+                min_val = param.GetFieldValue("min")
+                max_val = param.GetFieldValue("max")
+                def_val = param.GetFieldValue("default")
+                long_desc = param.GetFieldValue("long_desc")
+
+                name = name.replace("\n", " ")
+                result += "| %s | %s |" % (code, name)
+
+                if min_val is not None:
+                    result += "  %s |" % min_val
+                else:
+                    result += " |"
+
+                if max_val is not None:
+                    result += "  %s |" % max_val
+                else:
+                    result += " |"
+
+                if def_val is not None:
+                    result += "  %s |" % def_val
+                else:
+                    result += " |"
+
+                if long_desc is not None:
+                    long_desc = long_desc.replace("\n", " ")
+                    result += " %s |" % long_desc
+                else:
+                    result += " |"
+
+                result += "\n"
+            result += "\n"
+        self.output = result;
+
+    def Save(self, filename):
+        with codecs.open(filename, 'w', 'utf-8') as f:
+            f.write(self.output)
+
+    def SaveRpc(self, filename):
+        with codecs.open(filename, 'w', 'utf-8') as f:
+            f.write("""<?xml version='1.0'?>
+<methodCall>
+  <methodName>wiki.putPage</methodName>
+      <params>
+          <param> 
+            <value>
+              <string>:firmware:parameters</string>
+            </value>
+          </param>
+          <param> 
+            <value>
+              <string>""")
+            f.write(escape(self.output))
+            f.write("""</string>
+            </value>
+        </param>
+        <param> 
+            <value>
+                <name>sum</name>
+                <string>Updated parameters automagically from code.</string>
+            </value>
+        </param>
+    </params>
+</methodCall>""")

Tools/px4params/output_xml.py

@@ -1,8 +1,8 @@
-import output
 from xml.dom.minidom import getDOMImplementation
+import codecs
 
-class XMLOutput(output.Output):
-    def Generate(self, groups):
+class XMLOutput():
+    def __init__(self, groups):
         impl = getDOMImplementation()
         xml_document = impl.createDocument(None, "parameters", None)
         xml_parameters = xml_document.documentElement
@@ -19,4 +19,8 @@ class XMLOutput(output.Output):
                     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")
+        self.xml_document = xml_document
+
+    def Save(self, filename):
+        with codecs.open(filename, 'w', 'utf-8') as f:
+            self.xml_document.writexml(f, indent="    ", addindent="    ", newl="\n")

Tools/px4params/px_process_params.py

@@ -40,22 +40,28 @@
 #
 
 import scanner
-import parser
-import xmlout
-import dokuwikiout
+import srcparser
+import output_xml
+import output_dokuwiki_tables
+import output_dokuwiki_listings
 
 # Initialize parser
-prs = parser.Parser()
+prs = srcparser.Parser()
 
 # Scan directories, and parse the files
 sc = scanner.Scanner()
 sc.ScanDir("../../src", prs)
-output = prs.GetParamGroups()
+groups = prs.GetParamGroups()
 
 # Output into XML
-out = xmlout.XMLOutput()
-out.Save(output, "parameters.xml")
+out = output_xml.XMLOutput(groups)
+out.Save("parameters.xml")
 
-# Output into DokuWiki
-out = dokuwikiout.DokuWikiOutput()
-out.Save(output, "parameters.wiki")
+# Output to DokuWiki listings
+#out = output_dokuwiki_listings.DokuWikiListingsOutput(groups)
+#out.Save("parameters.wiki")
+
+# Output to DokuWiki tables
+out = output_dokuwiki_tables.DokuWikiTablesOutput(groups)
+out.Save("parameters.wiki")
+out.SaveRpc("parameters.wikirpc.xml")

Tools/px4params/scanner.py

@@ -1,5 +1,6 @@
 import os
 import re
+import codecs
 
 class Scanner(object):
     """
@@ -29,6 +30,6 @@ class Scanner(object):
         Scans provided file and passes its contents to the parser using
         parser.Parse method.
         """
-        with open(path, 'r') as f:
+        with codecs.open(path, 'r', 'utf-8') as f:
             contents = f.read()
         parser.Parse(contents)

Tools/px4params/srcparser.py

@@ -28,8 +28,7 @@ class ParameterGroup(object):
         state of the parser.
         """
         return sorted(self.params,
-                cmp=lambda x, y: cmp(x.GetFieldValue("code"),
-                    y.GetFieldValue("code")))
+                key=lambda x: x.GetFieldValue("code"))
 
 class Parameter(object):
     """
@@ -61,9 +60,10 @@ class Parameter(object):
         """
         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))
+        keys = self.fields.keys()
+        keys = sorted(keys)
+        keys = sorted(keys, key=lambda x: self.priority.get(x, 0), reverse=True)
+        return keys
 
     def GetFieldValue(self, code):
         """
@@ -197,7 +197,7 @@ class Parser(object):
                             if tag == "group":
                                 group = tags[tag]
                             elif tag not in self.valid_tags:
-                                sys.stderr.write("Skipping invalid"
+                                sys.stderr.write("Skipping invalid "
                                         "documentation tag: '%s'\n" % tag)
                             else:
                                 param.SetField(tag, tags[tag])
@@ -214,7 +214,7 @@ class Parser(object):
         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()))
+        groups = self.param_groups.values()
+        groups = sorted(groups, key=lambda x: x.GetName())
+        groups = sorted(groups, key=lambda x: self.priority.get(x.GetName(), 0), reverse=True)
+        return groups

Tools/px4params/xmlrpc.sh

@@ -2,4 +2,4 @@ python px_process_params.py
 
 rm cookies.txt
 curl --cookie cookies.txt --cookie-jar cookies.txt --user-agent Mozilla/4.0 --data "u=$XMLRPCUSER&p=$XMLRPCPASS" https://pixhawk.org/start?do=login
-curl -k --cookie cookies.txt -H "Content-Type: application/xml" -X POST --data-binary @parameters.wiki "https://pixhawk.org/lib/exe/xmlrpc.php"
+curl -k --cookie cookies.txt -H "Content-Type: application/xml" -X POST --data-binary @parameters.wikirpc.xml "https://pixhawk.org/lib/exe/xmlrpc.php"

nuttx-configs/px4fmu-v2/include/board.h

@@ -141,9 +141,9 @@
 
 #define STM32_APB2_TIM1_CLKIN   (2*STM32_PCLK2_FREQUENCY)
 #define STM32_APB2_TIM8_CLKIN   (2*STM32_PCLK2_FREQUENCY)
-#define STM32_APB2_TIM9_CLKIN   (2*STM32_PCLK1_FREQUENCY)
-#define STM32_APB2_TIM10_CLKIN  (2*STM32_PCLK1_FREQUENCY)
-#define STM32_APB2_TIM11_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB2_TIM9_CLKIN   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM10_CLKIN  (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM11_CLKIN  (2*STM32_PCLK2_FREQUENCY)
 
 /* Timer Frequencies, if APBx is set to 1, frequency is same to APBx
  * otherwise frequency is 2xAPBx. 

nuttx-configs/px4fmu-v2/nsh/defconfig

@@ -306,7 +306,7 @@ CONFIG_UART5_RXDMA=y
 # CONFIG_USART6_RS485 is not set
 CONFIG_USART6_RXDMA=y
 # CONFIG_UART7_RS485 is not set
-# CONFIG_UART7_RXDMA is not set
+CONFIG_UART7_RXDMA=y
 # CONFIG_UART8_RS485 is not set
 CONFIG_UART8_RXDMA=y
 CONFIG_SERIAL_DISABLE_REORDERING=y
@@ -539,8 +539,8 @@ CONFIG_SERIAL_NPOLLWAITERS=2
 # CONFIG_USART3_SERIAL_CONSOLE is not set
 # CONFIG_UART4_SERIAL_CONSOLE is not set
 # CONFIG_USART6_SERIAL_CONSOLE is not set
-# CONFIG_UART7_SERIAL_CONSOLE is not set
-CONFIG_UART8_SERIAL_CONSOLE=y
+CONFIG_UART7_SERIAL_CONSOLE=y
+# CONFIG_UART8_SERIAL_CONSOLE is not set
 # CONFIG_NO_SERIAL_CONSOLE is not set
 
 #

src/drivers/airspeed/airspeed.cpp

@@ -76,8 +76,8 @@
 
 #include <drivers/airspeed/airspeed.h>
 
-Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) :
-	I2C("Airspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000),
+Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char* path) :
+	I2C("Airspeed", path, bus, address, 100000),
 	_reports(nullptr),
 	_buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")),
 	_max_differential_pressure_pa(0),

src/drivers/airspeed/airspeed.h

@@ -90,7 +90,7 @@ static const int ERROR = -1;
 class __EXPORT Airspeed : public device::I2C
 {
 public:
-	Airspeed(int bus, int address, unsigned conversion_interval);
+	Airspeed(int bus, int address, unsigned conversion_interval, const char* path);
 	virtual ~Airspeed();
 
 	virtual int	init();

src/drivers/drv_pwm_output.h

@@ -160,7 +160,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 */
+#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/ets_airspeed/ets_airspeed.cpp

@@ -77,6 +77,7 @@
 
 /* I2C bus address */
 #define I2C_ADDRESS	0x75	/* 7-bit address. 8-bit address is 0xEA */
+#define ETS_PATH	"/dev/ets_airspeed"
 
 /* Register address */
 #define READ_CMD	0x07	/* Read the data */
@@ -93,7 +94,7 @@
 class ETSAirspeed : public Airspeed
 {
 public:
-	ETSAirspeed(int bus, int address = I2C_ADDRESS);
+	ETSAirspeed(int bus, int address = I2C_ADDRESS, const char* path = ETS_PATH);
 
 protected:
 
@@ -112,8 +113,8 @@ protected:
  */
 extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]);
 
-ETSAirspeed::ETSAirspeed(int bus, int address) : Airspeed(bus, address,
-	CONVERSION_INTERVAL)
+ETSAirspeed::ETSAirspeed(int bus, int address, const char* path) : Airspeed(bus, address,
+	CONVERSION_INTERVAL, path)
 {
 
 }

src/drivers/meas_airspeed/meas_airspeed.cpp

@@ -50,6 +50,7 @@
  *    - Interfacing to MEAS Digital Pressure Modules (http://www.meas-spec.com/downloads/Interfacing_to_MEAS_Digital_Pressure_Modules.pdf)
  */
 
+
 #include <nuttx/config.h>
 
 #include <drivers/device/i2c.h>
@@ -89,8 +90,10 @@
 
 /* I2C bus address is 1010001x */
 #define I2C_ADDRESS_MS4525DO	0x28	//0x51 /* 7-bit address. */
+#define PATH_MS4525		"/dev/ms4525"
 /* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */
 #define I2C_ADDRESS_MS5525DSO	0x77	//0x77/* 7-bit address, addr. pin pulled low */
+#define PATH_MS5525		"/dev/ms5525"
 
 /* Register address */
 #define ADDR_READ_MR			0x00	/* write to this address to start conversion */
@@ -101,7 +104,7 @@
 class MEASAirspeed : public Airspeed
 {
 public:
-	MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO);
+	MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO, const char* path = PATH_MS4525);
 
 protected:
 
@@ -120,8 +123,8 @@ protected:
  */
 extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]);
 
-MEASAirspeed::MEASAirspeed(int bus, int address) : Airspeed(bus, address,
-			CONVERSION_INTERVAL)
+MEASAirspeed::MEASAirspeed(int bus, int address, const char* path) : Airspeed(bus, address,
+			CONVERSION_INTERVAL, path)
 {
 
 }
@@ -304,7 +307,7 @@ start(int i2c_bus)
 		errx(1, "already started");
 
 	/* create the driver, try the MS4525DO first */
-	g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO);
+	g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO, PATH_MS4525);
 
 	/* check if the MS4525DO was instantiated */
 	if (g_dev == nullptr)
@@ -313,7 +316,7 @@ start(int i2c_bus)
 	/* try the MS5525DSO next if init fails */
 	if (OK != g_dev->Airspeed::init()) {
 		delete g_dev;
-		g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO);
+		g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO, PATH_MS5525);
 
 		/* check if the MS5525DSO was instantiated */
 		if (g_dev == nullptr)
@@ -386,7 +389,7 @@ test()
 		err(1, "immediate read failed");
 
 	warnx("single read");
-	warnx("diff pressure: %d pa", (double)report.differential_pressure_pa);
+	warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa);
 
 	/* start the sensor polling at 2Hz */
 	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
@@ -411,7 +414,7 @@ test()
 			err(1, "periodic read failed");
 
 		warnx("periodic read %u", i);
-		warnx("diff pressure: %d pa", report.differential_pressure_pa);
+		warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa);
 		warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature);
 	}
 

src/drivers/mkblctrl/mkblctrl.cpp

@@ -705,7 +705,7 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C)
 			Motor[i].State |= MOTOR_STATE_PRESENT_MASK; // set present bit;
 			foundMotorCount++;
 
-			if (Motor[i].MaxPWM == 250) {
+			if ((Motor[i].MaxPWM & 252) == 248) {
 				Motor[i].Version = BLCTRL_NEW;
 
 			} else {

src/drivers/mpu6000/mpu6000.cpp

@@ -1353,6 +1353,7 @@ MPU6000::print_info()
 MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) :
 	CDev("MPU6000_gyro", MPU_DEVICE_PATH_GYRO),
 	_parent(parent),
+	_gyro_topic(-1),
 	_gyro_class_instance(-1)
 {
 }

src/drivers/px4fmu/fmu.cpp

@@ -1714,7 +1714,7 @@ fmu_main(int argc, char *argv[])
 	}
 
 
-	fprintf(stderr, "FMU: unrecognised command, try:\n");
+	fprintf(stderr, "FMU: unrecognised command %s, try:\n", verb);
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
 	fprintf(stderr, "  mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n");
 #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)

src/drivers/px4io/px4io.cpp

@@ -244,8 +244,7 @@ private:
 	volatile int		_task;			///< worker task id
 	volatile bool		_task_should_exit;	///< worker terminate flag
 
-	int			_mavlink_fd;		///< mavlink file descriptor. This is opened by class instantiation and Doesn't appear to be usable in main thread.
-	int			_thread_mavlink_fd;	///< mavlink file descriptor for thread.
+	int			_mavlink_fd;		///< mavlink file descriptor.
 
 	perf_counter_t		_perf_update;		///<local performance counter for status updates
 	perf_counter_t		_perf_write;		///<local performance counter for PWM control writes
@@ -474,7 +473,6 @@ PX4IO::PX4IO(device::Device *interface) :
 	_task(-1),
 	_task_should_exit(false),
 	_mavlink_fd(-1),
-	_thread_mavlink_fd(-1),
 	_perf_update(perf_alloc(PC_ELAPSED, "io update")),
 	_perf_write(perf_alloc(PC_ELAPSED, "io write")),
 	_perf_chan_count(perf_alloc(PC_COUNT, "io rc #")),
@@ -507,9 +505,6 @@ PX4IO::PX4IO(device::Device *interface) :
 	/* we need this potentially before it could be set in task_main */
 	g_dev = this;
 
-	/* open MAVLink text channel */
-	_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
-
 	_debug_enabled = false;
 	_servorail_status.rssi_v = 0;
 }
@@ -785,7 +780,7 @@ PX4IO::task_main()
 	hrt_abstime poll_last = 0;
 	hrt_abstime orb_check_last = 0;
 
-	_thread_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
+	_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
 
 	/*
 	 * Subscribe to the appropriate PWM output topic based on whether we are the
@@ -880,6 +875,10 @@ PX4IO::task_main()
 			/* run at 5Hz */
 			orb_check_last = now;
 
+			/* try to claim the MAVLink log FD */
+			if (_mavlink_fd < 0)
+				_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
+
 			/* check updates on uORB topics and handle it */
 			bool updated = false;
 
@@ -1275,16 +1274,14 @@ void
 PX4IO::dsm_bind_ioctl(int dsmMode)
 {
 	if (!(_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
-		/* 0: dsm2, 1:dsmx */
-		if ((dsmMode == 0) || (dsmMode == 1)) {
-			mavlink_log_info(_thread_mavlink_fd, "[IO] binding dsm%s rx", (dsmMode == 0) ? "2" : ((dsmMode == 1) ? "x" : "x8"));
-			ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : ((dsmMode == 1) ? DSMX_BIND_PULSES : DSMX8_BIND_PULSES));
-		} else {
-			mavlink_log_info(_thread_mavlink_fd, "[IO] invalid dsm bind mode, bind request rejected");
-		}
+		mavlink_log_info(_mavlink_fd, "[IO] binding DSM%s RX", (dsmMode == 0) ? "2" : ((dsmMode == 1) ? "-X" : "-X8"));
+		int ret = ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : ((dsmMode == 1) ? DSMX_BIND_PULSES : DSMX8_BIND_PULSES));
+
+		if (ret)
+			mavlink_log_critical(_mavlink_fd, "binding failed.");
 
 	} else {
-		mavlink_log_info(_thread_mavlink_fd, "[IO] system armed, bind request rejected");
+		mavlink_log_info(_mavlink_fd, "[IO] system armed, bind request rejected");
 	}
 }
 
@@ -2115,14 +2112,24 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 		break;
 
 	case DSM_BIND_START:
-		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
-		usleep(500000);
-		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out);
-		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up);
-		usleep(72000);
-		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4));
-		usleep(50000);
-		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart);
+
+		/* only allow DSM2, DSM-X and DSM-X with more than 7 channels */
+		if (arg == DSM2_BIND_PULSES ||
+		    arg == DSMX_BIND_PULSES ||
+		    arg == DSMX8_BIND_PULSES) {
+			io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
+			usleep(500000);
+			io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out);
+			io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up);
+			usleep(72000);
+			io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4));
+			usleep(50000);
+			io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart);
+
+			ret = OK;
+		} else {
+			ret = -EINVAL;
+		}
 		break;
 
 	case DSM_BIND_POWER_UP:
@@ -2615,7 +2622,7 @@ bind(int argc, char *argv[])
 #endif
 
 	if (argc < 3)
-		errx(0, "needs argument, use dsm2 or dsmx");
+		errx(0, "needs argument, use dsm2, dsmx or dsmx8");
 
 	if (!strcmp(argv[2], "dsm2"))
 		pulses = DSM2_BIND_PULSES;
@@ -2624,7 +2631,7 @@ bind(int argc, char *argv[])
 	else if (!strcmp(argv[2], "dsmx8"))
 		pulses = DSMX8_BIND_PULSES;
 	else 
-		errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]);
+		errx(1, "unknown parameter %s, use dsm2, dsmx or dsmx8", argv[2]);
 	// Test for custom pulse parameter
 	if (argc > 3)
 		pulses = atoi(argv[3]);

src/lib/ecl/ecl.h

@@ -38,7 +38,6 @@
  */
 
 #include <drivers/drv_hrt.h>
-#include <geo/geo.h>
 
 #define ecl_absolute_time hrt_absolute_time
-#define ecl_elapsed_time hrt_elapsed_time
+#define ecl_elapsed_time hrt_elapsed_time

src/lib/ecl/l1/ecl_l1_pos_controller.cpp

@@ -38,6 +38,8 @@
  *
  */
 
+#include <float.h>
+
 #include "ecl_l1_pos_controller.h"
 
 float ECL_L1_Pos_Controller::nav_roll()
@@ -231,8 +233,15 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, con
 	/* calculate the vector from waypoint A to current position */
 	math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
 
-	/* store the normalized vector from waypoint A to current position */
-	math::Vector<2> vector_A_to_airplane_unit = (vector_A_to_airplane).normalized();
+	math::Vector<2> vector_A_to_airplane_unit;
+
+	/* prevent NaN when normalizing */
+	if (vector_A_to_airplane.length() > FLT_EPSILON) {
+		/* store the normalized vector from waypoint A to current position */
+		vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
+	} else {
+		vector_A_to_airplane_unit = vector_A_to_airplane;
+	}
 
 	/* calculate eta angle towards the loiter center */
 

src/lib/external_lgpl/tecs/tecs.cpp

@@ -3,13 +3,10 @@
 #include "tecs.h"
 #include <ecl/ecl.h>
 #include <systemlib/err.h>
+#include <geo/geo.h>
 
 using namespace math;
 
-#ifndef CONSTANTS_ONE_G
-#define CONSTANTS_ONE_G GRAVITY
-#endif
-
 /**
  * @file tecs.cpp
  *

src/lib/external_lgpl/tecs/tecs.h

@@ -28,16 +28,7 @@ class __EXPORT TECS
 {
 public:
 	TECS() :
-
-		_airspeed_enabled(false),
-		_throttle_slewrate(0.0f),
-		_climbOutDem(false),
-		_hgt_dem_prev(0.0f),
-		_hgt_dem_adj_last(0.0f),
-		_hgt_dem_in_old(0.0f),
-		_TAS_dem_last(0.0f),
-		_TAS_dem_adj(0.0f),
-		_TAS_dem(0.0f),
+		_pitch_dem(0.0f),
 		_integ1_state(0.0f),
 		_integ2_state(0.0f),
 		_integ3_state(0.0f),
@@ -45,8 +36,16 @@ public:
 		_integ5_state(0.0f),
 		_integ6_state(0.0f),
 		_integ7_state(0.0f),
-		_pitch_dem(0.0f),
 		_last_pitch_dem(0.0f),
+		_vel_dot(0.0f),
+		_TAS_dem(0.0f),
+		_TAS_dem_last(0.0f),
+		_hgt_dem_in_old(0.0f),
+		_hgt_dem_adj_last(0.0f),
+		_hgt_dem_prev(0.0f),
+		_TAS_dem_adj(0.0f),
+		_STEdotErrLast(0.0f),
+		_climbOutDem(false),
 		_SPE_dem(0.0f),
 		_SKE_dem(0.0f),
 		_SPEdot_dem(0.0f),
@@ -55,9 +54,9 @@ public:
 		_SKE_est(0.0f),
 		_SPEdot(0.0f),
 		_SKEdot(0.0f),
-		_vel_dot(0.0f),
-		_STEdotErrLast(0.0f) {
-
+		_airspeed_enabled(false),
+		_throttle_slewrate(0.0f)
+	{
 	}
 
 	bool airspeed_sensor_enabled() {

src/lib/launchdetection/CatapultLaunchMethod.cpp

@@ -42,7 +42,7 @@
 #include <systemlib/err.h>
 
 CatapultLaunchMethod::CatapultLaunchMethod() :
-	last_timestamp(0),
+	last_timestamp(hrt_absolute_time()),
 	integrator(0.0f),
 	launchDetected(false),
 	threshold_accel(NULL, "LAUN_CAT_A", false),
@@ -88,3 +88,9 @@ void CatapultLaunchMethod::updateParams()
 	threshold_accel.update();
 	threshold_time.update();
 }
+
+void CatapultLaunchMethod::reset()
+{
+	integrator = 0.0f;
+	launchDetected = false;
+}

src/lib/launchdetection/CatapultLaunchMethod.h

@@ -55,11 +55,10 @@ public:
 	void update(float accel_x);
 	bool getLaunchDetected();
 	void updateParams();
+	void reset();
 
 private:
 	hrt_abstime last_timestamp;
-//	float threshold_accel_raw;
-//	float threshold_time;
 	float integrator;
 	bool launchDetected;
 

src/lib/launchdetection/LaunchDetector.cpp

@@ -59,6 +59,12 @@ LaunchDetector::~LaunchDetector()
 
 }
 
+void LaunchDetector::reset()
+{
+	/* Reset all detectors */
+	launchMethods[0]->reset();
+}
+
 void LaunchDetector::update(float accel_x)
 {
 	if (launchdetection_on.get() == 1) {

src/lib/launchdetection/LaunchDetector.h

@@ -53,6 +53,7 @@ class __EXPORT LaunchDetector
 public:
 	LaunchDetector();
 	~LaunchDetector();
+	void reset();
 
 	void update(float accel_x);
 	bool getLaunchDetected();

src/lib/launchdetection/LaunchMethod.h

@@ -47,6 +47,7 @@ public:
 	virtual void update(float accel_x) = 0;
 	virtual bool getLaunchDetected() = 0;
 	virtual void updateParams() = 0;
+	virtual void reset() = 0;
 protected:
 private:
 };

src/lib/launchdetection/launchdetection_params.c

@@ -45,28 +45,46 @@
 #include <systemlib/param/param.h>
 
 /*
- * Launch detection parameters, accessible via MAVLink
+ * Catapult launch detection parameters, accessible via MAVLink
  *
  */
 
-/* Catapult Launch detection */
-
-// @DisplayName		Switch to enable launchdetection
-// @Description		if set to 1 launchdetection is enabled
-// @Range		0 or 1
+/**
+ * Enable launch detection.
+ *
+ * @min 0
+ * @max 1
+ * @group Launch detection
+ */
 PARAM_DEFINE_INT32(LAUN_ALL_ON, 0);
 
-// @DisplayName		Catapult Accelerometer Threshold
-// @Description		LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection
-// @Range		> 0
+/**
+ * Catapult accelerometer theshold.
+ *
+ * LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection.
+ *
+ * @min 0
+ * @group Launch detection
+ */
 PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f);
 
-// @DisplayName		Catapult Time Threshold
-// @Description		LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection
-// @Range		> 0, in seconds
+/**
+ * Catapult time theshold.
+ *
+ * LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection.
+ *
+ * @min 0
+ * @group Launch detection
+ */
 PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f);
 
-// @DisplayName		Throttle setting while detecting the launch
-// @Description		The throttle is set to this value while the system is waiting for the takeoff
-// @Range		0 to 1
+/**
+ * Throttle setting while detecting launch.
+ *
+ * The throttle is set to this value while the system is waiting for the take-off.
+ *
+ * @min 0
+ * @max 1
+ * @group Launch detection
+ */
 PARAM_DEFINE_FLOAT(LAUN_THR_PRE, 0.0f);

src/modules/commander/accelerometer_calibration.cpp

@@ -311,7 +311,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
 				 (double)accel_ref[orient][2]);
 
 		data_collected[orient] = true;
-		tune_neutral();
+		tune_neutral(true);
 	}
 
 	close(sensor_combined_sub);

src/modules/commander/airspeed_calibration.cpp

@@ -142,7 +142,7 @@ int do_airspeed_calibration(int mavlink_fd)
 		}
 
 		mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
-		tune_neutral();
+		tune_neutral(true);
 		close(diff_pres_sub);
 		return OK;
 

src/modules/commander/commander.cpp

@@ -152,6 +152,7 @@ static uint64_t last_print_mode_reject_time = 0;
 static bool on_usb_power = false;
 
 static float takeoff_alt = 5.0f;
+static int parachute_enabled = 0;
 
 static struct vehicle_status_s status;
 static struct actuator_armed_s armed;
@@ -429,7 +430,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			arming_res = TRANSITION_NOT_CHANGED;
 
 			if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
-				if ((safety->safety_switch_available && !safety->safety_off) && status->hil_state == HIL_STATE_OFF) {
+				if (safety->safety_switch_available && !safety->safety_off && status->hil_state == HIL_STATE_OFF) {
 					print_reject_arm("NOT ARMING: Press safety switch first.");
 					arming_res = TRANSITION_DENIED;
 
@@ -515,7 +516,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			transition_result_t arming_res = TRANSITION_NOT_CHANGED;
 
 			if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) {
-				if (safety->safety_switch_available && !safety->safety_off) {
+				if (safety->safety_switch_available && !safety->safety_off && status->hil_state == HIL_STATE_OFF) {
 					print_reject_arm("NOT ARMING: Press safety switch first.");
 					arming_res = TRANSITION_DENIED;
 
@@ -563,7 +564,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		/* Flight termination */
 	case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
 
-			if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
+			//XXX: to enable the parachute, a param needs to be set
+			//xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
+			if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) {
 				transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
 				result = VEHICLE_CMD_RESULT_ACCEPTED;
 				ret = true;
@@ -607,7 +610,6 @@ int commander_thread_main(int argc, char *argv[])
 	/* not yet initialized */
 	commander_initialized = false;
 
-	bool battery_tune_played = false;
 	bool arm_tune_played = false;
 
 	/* set parameters */
@@ -615,6 +617,7 @@ int commander_thread_main(int argc, char *argv[])
 	param_t _param_system_id = param_find("MAV_SYS_ID");
 	param_t _param_component_id = param_find("MAV_COMP_ID");
 	param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT");
+	param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN");
 
 	/* welcome user */
 	warnx("starting");
@@ -860,10 +863,10 @@ int commander_thread_main(int argc, char *argv[])
 
 				/* re-check RC calibration */
 				rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
-
-				/* navigation parameters */
-				param_get(_param_takeoff_alt, &takeoff_alt);
 			}
+			/* navigation parameters */
+			param_get(_param_takeoff_alt, &takeoff_alt);
+			param_get(_param_enable_parachute, &parachute_enabled);
 		}
 
 		orb_check(sp_man_sub, &updated);
@@ -898,11 +901,13 @@ int commander_thread_main(int argc, char *argv[])
 		if (updated) {
 			orb_copy(ORB_ID(safety), safety_sub, &safety);
 
-			// XXX this would be the right approach to do it, but do we *WANT* this?
-			// /* disarm if safety is now on and still armed */
-			// if (safety.safety_switch_available && !safety.safety_off) {
-			// 	(void)arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
-			// }
+			/* disarm if safety is now on and still armed */
+			if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) {
+				arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
+				if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) {
+					mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch");
+				}
+			}
 		}
 
 		/* update global position estimate */
@@ -957,7 +962,7 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(battery_status), battery_sub, &battery);
 
 			/* only consider battery voltage if system has been running 2s and battery voltage is valid */
-			if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
+			if (status.hil_state == HIL_STATE_OFF && hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
 				status.battery_voltage = battery.voltage_filtered_v;
 				status.battery_current = battery.current_a;
 				status.condition_battery_voltage_valid = true;
@@ -1020,14 +1025,12 @@ int commander_thread_main(int argc, char *argv[])
 			mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY");
 			status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
 			status_changed = true;
-			battery_tune_played = false;
 
 		} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
 			/* critical battery voltage, this is rather an emergency, change state machine */
 			critical_battery_voltage_actions_done = true;
 			mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY");
 			status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
-			battery_tune_played = false;
 
 			if (armed.armed) {
 				arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
@@ -1101,7 +1104,7 @@ int commander_thread_main(int argc, char *argv[])
 
 				/* mark home position as set */
 				status.condition_home_position_valid = true;
-				tune_positive();
+				tune_positive(true);
 			}
 		}
 
@@ -1152,7 +1155,7 @@ int commander_thread_main(int argc, char *argv[])
 			if (status.arming_state == ARMING_STATE_STANDBY &&
 			    sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
 				if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
-					if (safety.safety_switch_available && !safety.safety_off) {
+					if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) {
 						print_reject_arm("NOT ARMING: Press safety switch first.");
 
 					} else if (status.main_state != MAIN_STATE_MANUAL) {
@@ -1196,8 +1199,9 @@ int commander_thread_main(int argc, char *argv[])
 			/* evaluate the main state machine according to mode switches */
 			res = set_main_state_rc(&status);
 
+			/* play tune on mode change only if armed, blink LED always */
 			if (res == TRANSITION_CHANGED) {
-				tune_positive();
+				tune_positive(armed.armed);
 
 			} else if (res == TRANSITION_DENIED) {
 				/* DENIED here indicates bug in the commander */
@@ -1251,9 +1255,9 @@ int commander_thread_main(int argc, char *argv[])
 
 		// TODO remove this hack
 		/* flight termination in manual mode if assisted switch is on easy position */
-		if (!status.is_rotary_wing && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
+		if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
 			if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
-				tune_positive();
+				tune_positive(armed.armed);
 			}
 		}
 
@@ -1308,26 +1312,23 @@ int commander_thread_main(int argc, char *argv[])
 		/* play arming and battery warning tunes */
 		if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available && safety.safety_off))) {
 			/* play tune when armed */
-			if (tune_arm() == OK)
-				arm_tune_played = true;
-
-		} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
-			/* play tune on battery warning */
-			if (tune_low_bat() == OK)
-				battery_tune_played = true;
+			set_tune(TONE_ARMING_WARNING_TUNE);
+			arm_tune_played = true;
 
 		} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
 			/* play tune on battery critical */
-			if (tune_critical_bat() == OK)
-				battery_tune_played = true;
+			set_tune(TONE_BATTERY_WARNING_FAST_TUNE);
 
-		} else if (battery_tune_played) {
-			tune_stop();
-			battery_tune_played = false;
+		} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe_state != FAILSAFE_STATE_NORMAL) {
+			/* play tune on battery warning or failsafe */
+			set_tune(TONE_BATTERY_WARNING_SLOW_TUNE);
+
+		} else {
+			set_tune(TONE_STOP_TUNE);
 		}
 
 		/* reset arm_tune_played when disarmed */
-		if (status.arming_state != ARMING_STATE_ARMED || (safety.safety_switch_available && !safety.safety_off)) {
+		if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) {
 			arm_tune_played = false;
 		}
 
@@ -1422,11 +1423,8 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
 
 		if (set_normal_color) {
 			/* set color */
-			if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) {
-				if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
-					rgbled_set_color(RGBLED_COLOR_AMBER);
-				}
-
+			if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe_state != FAILSAFE_STATE_NORMAL) {
+				rgbled_set_color(RGBLED_COLOR_AMBER);
 				/* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */
 
 			} else {
@@ -1693,15 +1691,9 @@ print_reject_mode(struct vehicle_status_s *status, const char *msg)
 		sprintf(s, "#audio: REJECT %s", msg);
 		mavlink_log_critical(mavlink_fd, s);
 
-		// only buzz if armed, because else we're driving people nuts indoors
-		// they really need to look at the leds as well.
-		if (status->arming_state == ARMING_STATE_ARMED) {
-			tune_negative();
-		} else {
-
-			// Always show the led indication
-			led_negative();
-		}
+		/* only buzz if armed, because else we're driving people nuts indoors
+		they really need to look at the leds as well. */
+		tune_negative(armed.armed);
 	}
 }
 
@@ -1715,7 +1707,7 @@ print_reject_arm(const char *msg)
 		char s[80];
 		sprintf(s, "#audio: %s", msg);
 		mavlink_log_critical(mavlink_fd, s);
-		tune_negative();
+		tune_negative(true);
 	}
 }
 
@@ -1723,27 +1715,27 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
 {
 	switch (result) {
 	case VEHICLE_CMD_RESULT_ACCEPTED:
-			tune_positive();
+			tune_positive(true);
 		break;
 
 	case VEHICLE_CMD_RESULT_DENIED:
 		mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd.command);
-		tune_negative();
+		tune_negative(true);
 		break;
 
 	case VEHICLE_CMD_RESULT_FAILED:
 		mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd.command);
-		tune_negative();
+		tune_negative(true);
 		break;
 
 	case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
 		mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd.command);
-		tune_negative();
+		tune_negative(true);
 		break;
 
 	case VEHICLE_CMD_RESULT_UNSUPPORTED:
 		mavlink_log_critical(mavlink_fd, "#audio: command unsupported: %u", cmd.command);
-		tune_negative();
+		tune_negative(true);
 		break;
 
 	default:
@@ -1883,9 +1875,9 @@ void *commander_low_prio_loop(void *arg)
 				}
 
 				if (calib_ret == OK)
-					tune_positive();
+					tune_positive(true);
 				else
-					tune_negative();
+					tune_negative(true);
 
 				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
 

src/modules/commander/commander_helper.cpp

@@ -45,6 +45,7 @@
 #include <stdbool.h>
 #include <fcntl.h>
 #include <math.h>
+#include <string.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
@@ -81,11 +82,22 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status)
 	       || (current_status->system_type == VEHICLE_TYPE_COAXIAL);
 }
 
-static int buzzer;
-static hrt_abstime blink_msg_end;
+static int buzzer = -1;
+static hrt_abstime blink_msg_end = 0;	// end time for currently blinking LED message, 0 if no blink message
+static hrt_abstime tune_end = 0;		// end time of currently played tune, 0 for repeating tunes or silence
+static int tune_current = TONE_STOP_TUNE;		// currently playing tune, can be interrupted after tune_end
+static unsigned int tune_durations[TONE_NUMBER_OF_TUNES];
 
 int buzzer_init()
 {
+	tune_end = 0;
+	tune_current = 0;
+	memset(tune_durations, 0, sizeof(tune_durations));
+	tune_durations[TONE_NOTIFY_POSITIVE_TUNE] = 800000;
+	tune_durations[TONE_NOTIFY_NEGATIVE_TUNE] = 900000;
+	tune_durations[TONE_NOTIFY_NEUTRAL_TUNE] = 500000;
+	tune_durations[TONE_ARMING_WARNING_TUNE] = 3000000;
+
 	buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY);
 
 	if (buzzer < 0) {
@@ -101,58 +113,60 @@ void buzzer_deinit()
 	close(buzzer);
 }
 
-void tune_error()
-{
-	ioctl(buzzer, TONE_SET_ALARM, TONE_ERROR_TUNE);
+void set_tune(int tune) {
+	unsigned int new_tune_duration = tune_durations[tune];
+	/* don't interrupt currently playing non-repeating tune by repeating */
+	if (tune_end == 0 || new_tune_duration != 0 || hrt_absolute_time() > tune_end) {
+		/* allow interrupting current non-repeating tune by the same tune */
+		if (tune != tune_current || new_tune_duration != 0) {
+			ioctl(buzzer, TONE_SET_ALARM, tune);
+		}
+		tune_current = tune;
+		if (new_tune_duration != 0) {
+			tune_end = hrt_absolute_time() + new_tune_duration;
+		} else {
+			tune_end = 0;
+		}
+	}
 }
 
-void tune_positive()
+/**
+ * Blink green LED and play positive tune (if use_buzzer == true).
+ */
+void tune_positive(bool use_buzzer)
 {
 	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
 	rgbled_set_color(RGBLED_COLOR_GREEN);
 	rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
-	ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_POSITIVE_TUNE);
+	if (use_buzzer) {
+		set_tune(TONE_NOTIFY_POSITIVE_TUNE);
+	}
 }
 
-void tune_neutral()
+/**
+ * Blink white LED and play neutral tune (if use_buzzer == true).
+ */
+void tune_neutral(bool use_buzzer)
 {
 	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
 	rgbled_set_color(RGBLED_COLOR_WHITE);
 	rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
-	ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE);
+	if (use_buzzer) {
+		set_tune(TONE_NOTIFY_NEUTRAL_TUNE);
+	}
 }
 
-void tune_negative()
-{
-	led_negative();
-	ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE);
-}
-
-void led_negative()
+/**
+ * Blink red LED and play negative tune (if use_buzzer == true).
+ */
+void tune_negative(bool use_buzzer)
 {
 	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
 	rgbled_set_color(RGBLED_COLOR_RED);
 	rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
-}
-
-int tune_arm()
-{
-	return ioctl(buzzer, TONE_SET_ALARM, TONE_ARMING_WARNING_TUNE);
-}
-
-int tune_low_bat()
-{
-	return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_SLOW_TUNE);
-}
-
-int tune_critical_bat()
-{
-	return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_FAST_TUNE);
-}
-
-void tune_stop()
-{
-	ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE);
+	if (use_buzzer) {
+		set_tune(TONE_NOTIFY_NEGATIVE_TUNE);
+	}
 }
 
 int blink_msg_state()
@@ -161,6 +175,7 @@ int blink_msg_state()
 		return 0;
 
 	} else if (hrt_absolute_time() > blink_msg_end) {
+		blink_msg_end = 0;
 		return 2;
 
 	} else {
@@ -168,8 +183,8 @@ int blink_msg_state()
 	}
 }
 
-static int leds;
-static int rgbleds;
+static int leds = -1;
+static int rgbleds = -1;
 
 int led_init()
 {

src/modules/commander/commander_helper.h

@@ -54,16 +54,10 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status);
 int buzzer_init(void);
 void buzzer_deinit(void);
 
-void tune_error(void);
-void tune_positive(void);
-void tune_neutral(void);
-void tune_negative(void);
-int tune_arm(void);
-int tune_low_bat(void);
-int tune_critical_bat(void);
-void tune_stop(void);
-
-void led_negative();
+void set_tune(int tune);
+void tune_positive(bool use_buzzer);
+void tune_neutral(bool use_buzzer);
+void tune_negative(bool use_buzzer);
 
 int blink_msg_state();
 

src/modules/commander/commander_params.c

@@ -48,7 +48,39 @@
 PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
 PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
 PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
+
+/**
+ * Empty cell voltage.
+ *
+ * Defines the voltage where a single cell of the battery is considered empty.
+ *
+ * @group Battery Calibration
+ */
 PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f);
+
+/**
+ * Full cell voltage.
+ *
+ * Defines the voltage where a single cell of the battery is considered full.
+ *
+ * @group Battery Calibration
+ */
 PARAM_DEFINE_FLOAT(BAT_V_FULL, 3.9f);
+
+/**
+ * Number of cells.
+ *
+ * Defines the number of cells the attached battery consists of.
+ *
+ * @group Battery Calibration
+ */
 PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
+
+/**
+ * Battery capacity.
+ *
+ * Defines the capacity of the attached battery.
+ *
+ * @group Battery Calibration
+ */
 PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -130,23 +130,23 @@ private:
 	int		_att_sub;			/**< vehicle attitude subscription */
 	int		_attitude_sub;			/**< raw rc channels data subscription */
 	int		_airspeed_sub;			/**< airspeed subscription */
-	int		_control_mode_sub;			/**< vehicle status subscription */
+	int		_control_mode_sub;		/**< vehicle status subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
 	int 		_manual_control_sub;		/**< notification of manual control updates */
-	int		_sensor_combined_sub;			/**< for body frame accelerations */
+	int		_sensor_combined_sub;		/**< for body frame accelerations */
 
 	orb_advert_t	_attitude_sp_pub;		/**< attitude setpoint */
 	orb_advert_t	_nav_capabilities_pub;		/**< navigation capabilities publication */
 
-	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
-	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude setpoint */
-	struct navigation_capabilities_s		_nav_capabilities;	/**< navigation capabilities */
-	struct manual_control_setpoint_s		_manual;		/**< r/c channel data */
-	struct airspeed_s				_airspeed;		/**< airspeed */
-	struct vehicle_control_mode_s				_control_mode;		/**< vehicle status */
-	struct vehicle_global_position_s		_global_pos;		/**< global vehicle position */
-	struct position_setpoint_triplet_s			_pos_sp_triplet;		/**< triplet of mission items */
-	struct sensor_combined_s				_sensor_combined;			/**< for body frame accelerations */
+	struct vehicle_attitude_s			_att;				/**< vehicle attitude */
+	struct vehicle_attitude_setpoint_s		_att_sp;			/**< vehicle attitude setpoint */
+	struct navigation_capabilities_s		_nav_capabilities;		/**< navigation capabilities */
+	struct manual_control_setpoint_s		_manual;			/**< r/c channel data */
+	struct airspeed_s				_airspeed;			/**< airspeed */
+	struct vehicle_control_mode_s			_control_mode;			/**< vehicle status */
+	struct vehicle_global_position_s		_global_pos;			/**< global vehicle position */
+	struct position_setpoint_triplet_s		_pos_sp_triplet;		/**< triplet of mission items */
+	struct sensor_combined_s			_sensor_combined;		/**< for body frame accelerations */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
@@ -154,13 +154,13 @@ private:
 
 	/** manual control states */
 	float		_seatbelt_hold_heading;		/**< heading the system should hold in seatbelt mode */
-	float		_loiter_hold_lat;
-	float		_loiter_hold_lon;
+	double		_loiter_hold_lat;
+	double		_loiter_hold_lon;
 	float		_loiter_hold_alt;
 	bool		_loiter_hold;
 
-	float		_launch_lat;
-	float		_launch_lon;
+	double		_launch_lat;
+	double		_launch_lon;
 	float		_launch_alt;
 	bool		_launch_valid;
 
@@ -176,6 +176,8 @@ private:
 	bool launch_detected;
 	bool usePreTakeoffThrust;
 
+	bool last_manual;				///< true if the last iteration was in manual mode (used to determine when a reset is needed)
+
 	/* Landingslope object */
 	Landingslope landingslope;
 
@@ -192,7 +194,7 @@ private:
 	uint64_t _airspeed_last_valid;			///< last time airspeed was valid. Used to detect sensor failures
 	float _groundspeed_undershoot;			///< ground speed error to min. speed in m/s
 	bool _global_pos_valid;				///< global position is valid
-	math::Matrix<3, 3> _R_nb;				///< current attitude
+	math::Matrix<3, 3> _R_nb;			///< current attitude
 
 	ECL_L1_Pos_Controller				_l1_control;
 	TECS						_tecs;
@@ -233,7 +235,6 @@ private:
 		float speedrate_p;
 
 		float land_slope_angle;
-		float land_slope_length;
 		float land_H1_virt;
 		float land_flare_alt_relative;
 		float land_thrust_lim_alt_relative;
@@ -278,7 +279,6 @@ private:
 		param_t speedrate_p;
 
 		param_t land_slope_angle;
-		param_t land_slope_length;
 		param_t land_H1_virt;
 		param_t land_flare_alt_relative;
 		param_t land_thrust_lim_alt_relative;
@@ -346,6 +346,16 @@ private:
 	 * Main sensor collection task.
 	 */
 	void		task_main() __attribute__((noreturn));
+
+	/*
+	 * Reset takeoff state
+	 */
+	int reset_takeoff_state();
+
+	/*
+	 * Reset landing state
+	 */
+	int reset_landing_state();
 };
 
 namespace l1_control
@@ -362,6 +372,7 @@ FixedwingPositionControl	*g_control;
 
 FixedwingPositionControl::FixedwingPositionControl() :
 
+	_mavlink_fd(-1),
 	_task_should_exit(false),
 	_control_task(-1),
 
@@ -380,38 +391,34 @@ FixedwingPositionControl::FixedwingPositionControl() :
 
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
+
 /* states */
 	_setpoint_valid(false),
 	_loiter_hold(false),
-	_airspeed_error(0.0f),
-	_airspeed_valid(false),
-	_groundspeed_undershoot(0.0f),
-	_global_pos_valid(false),
 	land_noreturn_horizontal(false),
 	land_noreturn_vertical(false),
 	land_stayonground(false),
 	land_motor_lim(false),
 	land_onslope(false),
-	flare_curve_alt_last(0.0f),
-	_mavlink_fd(-1),
-	launchDetector(),
 	launch_detected(false),
-	usePreTakeoffThrust(false)
+	last_manual(false),
+	usePreTakeoffThrust(false),
+	flare_curve_alt_last(0.0f),
+	launchDetector(),
+	_airspeed_error(0.0f),
+	_airspeed_valid(false),
+	_groundspeed_undershoot(0.0f),
+	_global_pos_valid(false),
+	_att(),
+	_att_sp(),
+	_nav_capabilities(),
+	_manual(),
+	_airspeed(),
+	_control_mode(),
+	_global_pos(),
+	_pos_sp_triplet(),
+	_sensor_combined()
 {
-	/* safely initialize structs */
-	vehicle_attitude_s			_att = {0};
-	vehicle_attitude_setpoint_s		_att_sp = {0};
-	navigation_capabilities_s		_nav_capabilities = {0};
-	manual_control_setpoint_s		_manual = {0};
-	airspeed_s				_airspeed = {0};
-	vehicle_control_mode_s			_control_mode = {0};
-	vehicle_global_position_s		_global_pos = {0};
-	position_setpoint_triplet_s			_pos_sp_triplet = {0};
-	sensor_combined_s				_sensor_combined = {0};
-
-
-
-
 	_nav_capabilities.turn_distance = 0.0f;
 
 	_parameter_handles.l1_period = param_find("FW_L1_PERIOD");
@@ -431,7 +438,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX");
 
 	_parameter_handles.land_slope_angle = param_find("FW_LND_ANG");
-	_parameter_handles.land_slope_length = param_find("FW_LND_SLLR");
 	_parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT");
 	_parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT");
 	_parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT");
@@ -520,7 +526,6 @@ FixedwingPositionControl::parameters_update()
 	param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p));
 
 	param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle));
-	param_get(_parameter_handles.land_slope_length, &(_parameters.land_slope_length));
 	param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt));
 	param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative));
 	param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative));
@@ -587,8 +592,8 @@ FixedwingPositionControl::vehicle_control_mode_poll()
 		orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
 
 		if (!was_armed && _control_mode.flag_armed) {
-			_launch_lat = _global_pos.lat / 1e7f;
-			_launch_lon = _global_pos.lon / 1e7f;
+			_launch_lat = _global_pos.lat;
+			_launch_lon = _global_pos.lon;
 			_launch_alt = _global_pos.alt;
 			_launch_valid = true;
 		}
@@ -703,7 +708,7 @@ void
 FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet)
 {
 
-	if (_global_pos_valid) {
+	if (_global_pos_valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) {
 
 		/* rotate ground speed vector with current attitude */
 		math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
@@ -889,8 +894,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			float airspeed_land = 1.3f * _parameters.airspeed_min;
 			float airspeed_approach = 1.3f * _parameters.airspeed_min;
 
-			float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)) * _parameters.land_slope_length;
+			/* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
+			float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
 			float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt);
+
 			float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
 			float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt);
 
@@ -916,7 +923,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				float flare_curve_alt = landingslope.getFlareCurveAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt);
 
 				/* avoid climbout */
-				if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground)
+				if ((flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical) || land_stayonground)
 				{
 					flare_curve_alt = pos_sp_triplet.current.alt;
 					land_stayonground = true;
@@ -935,38 +942,24 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				//warnx("Landing:  flare, _global_pos.alt  %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance);
 
 				flare_curve_alt_last = flare_curve_alt;
-
-			} else if (wp_distance < L_wp_distance) {
-
-				/* minimize speed to approach speed, stay on landing slope */
-				_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, landing_slope_alt_desired, calculate_target_airspeed(airspeed_approach),
-							    _airspeed.indicated_airspeed_m_s, eas2tas,
-							    false, flare_pitch_angle_rad,
-							    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-							    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
-				//warnx("Landing: stay on slope, alt_desired: %.1f (wp_distance: %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f, d1 %.1f, flare_length %.1f", landing_slope_alt_desired, wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement, d1, flare_length);
-
-				if (!land_onslope) {
-
-					mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope");
-					land_onslope = true;
-				}
-
 			} else {
 
 				 /* intersect glide slope:
-				 * if current position is higher or within 10m of slope follow the glide slope
-				 * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope
-				 * */
+				  * minimize speed to approach speed
+				  * if current position is higher or within 10m of slope follow the glide slope
+				  * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope
+				  * */
 				float altitude_desired = _global_pos.alt;
 				if (_global_pos.alt > landing_slope_alt_desired - 10.0f) {
 					/* stay on slope */
 					altitude_desired = landing_slope_alt_desired;
-					//warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement);
+					if (!land_onslope) {
+						mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope");
+						land_onslope = true;
+					}
 				} else {
 					/* continue horizontally */
 					altitude_desired =  math::max(_global_pos.alt, L_altitude);
-					//warnx("Landing: before L,continue at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance);
 				}
 
 				_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach),
@@ -1042,19 +1035,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		// mission is active
 		_loiter_hold = false;
 
-		/* reset land state */
+		/* reset landing state */
 		if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) {
-			land_noreturn_horizontal = false;
-			land_noreturn_vertical = false;
-			land_stayonground = false;
-			land_motor_lim = false;
-			land_onslope = false;
+			reset_landing_state();
 		}
 
 		/* reset takeoff/launch state */
 		if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) {
-			launch_detected = false;
-			usePreTakeoffThrust = false;
+			reset_takeoff_state();
 		}
 
 		if (was_circle_mode && !_l1_control.circle_mode()) {
@@ -1074,13 +1062,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			_seatbelt_hold_heading = _att.yaw + _manual.yaw;
 		}
 
-		/* climb out control */
-		bool climb_out = false;
+		//XXX not used
 
-		/* user wants to climb out */
-		if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
-			climb_out = true;
-		}
+		/* climb out control */
+//		bool climb_out = false;
+//
+//		/* user wants to climb out */
+//		if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
+//			climb_out = true;
+//		}
 
 		/* if in seatbelt mode, set airspeed based on manual control */
 
@@ -1149,6 +1139,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 		/* no flight mode applies, do not publish an attitude setpoint */
 		setpoint = false;
+
+		/* reset landing and takeoff state */
+		if (!last_manual) {
+			reset_landing_state();
+			reset_takeoff_state();
+		}
 	}
 
 	if (usePreTakeoffThrust) {
@@ -1159,6 +1155,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	}
 	_att_sp.pitch_body = _tecs.get_pitch_demand();
 
+	if (_control_mode.flag_control_position_enabled) {
+		last_manual = false;
+	} else {
+		last_manual = true;
+	}
+
 
 	return setpoint;
 }
@@ -1287,7 +1289,7 @@ FixedwingPositionControl::task_main()
 				float turn_distance = _l1_control.switch_distance(100.0f);
 
 				/* lazily publish navigation capabilities */
-				if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) {
+				if (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON && turn_distance > 0) {
 
 					/* set new turn distance */
 					_nav_capabilities.turn_distance = turn_distance;
@@ -1309,6 +1311,22 @@ FixedwingPositionControl::task_main()
 	_exit(0);
 }
 
+int FixedwingPositionControl::reset_takeoff_state()
+{
+	launch_detected = false;
+	usePreTakeoffThrust = false;
+	launchDetector.reset();
+}
+
+int FixedwingPositionControl::reset_landing_state()
+{
+	land_noreturn_horizontal = false;
+	land_noreturn_vertical = false;
+	land_stayonground = false;
+	land_motor_lim = false;
+	land_onslope = false;
+}
+
 int
 FixedwingPositionControl::start()
 {

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -40,12 +40,10 @@
  */
 
 #include <nuttx/config.h>
-
 #include <systemlib/param/param.h>
 
 /*
  * Controller parameters, accessible via MAVLink
- *
  */
 
 /**
@@ -119,58 +117,261 @@ PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f);
  */
 PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f);
 
-
+/**
+ * Controller roll limit
+ *
+ * The maximum roll the controller will output.
+ *
+ * @unit degrees
+ * @min 0.0
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f);
 
-
-PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
-
-
+/**
+ * Throttle limit max
+ *
+ * This is the maximum throttle % that can be used by the controller. 
+ * For overpowered aircraft, this should be reduced to a value that 
+ * provides sufficient thrust to climb at the maximum pitch angle PTCH_MAX.
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f);
 
+/**
+ * Throttle limit min
+ *
+ * This is the minimum throttle % that can be used by the controller. 
+ * For electric aircraft this will normally be set to zero, but can be set 
+ * to a small non-zero value if a folding prop is fitted to prevent the 
+ * prop from folding and unfolding repeatedly in-flight or to provide 
+ * some aerodynamic drag from a turning prop to improve the descent rate.
+ *
+ * For aircraft with internal combustion engine this parameter should be set
+ * for desired idle rpm.
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
+
+/**
+ * Throttle limit value before flare
+ *
+ * This throttle value will be set as throttle limit at FW_LND_TLALT, 
+ * before arcraft will flare.
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f);
 
+/**
+ * Maximum climb rate
+ *
+ * This is the best climb rate that the aircraft can achieve with 
+ * the throttle set to THR_MAX and the airspeed set to the 
+ * default value. For electric aircraft make sure this number can be 
+ * achieved towards the end of flight when the battery voltage has reduced. 
+ * The setting of this parameter can be checked by commanding a positive 
+ * altitude change of 100m in loiter, RTL or guided mode. If the throttle 
+ * required to climb is close to THR_MAX and the aircraft is maintaining 
+ * airspeed, then this parameter is set correctly. If the airspeed starts 
+ * to reduce, then the parameter is set to high, and if the throttle 
+ * demand required to climb and maintain speed is noticeably less than 
+ * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or 
+ * FW_THR_MAX reduced.
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);
 
-
+/**
+ * Minimum descent rate
+ *
+ * This is the sink rate of the aircraft with the throttle 
+ * set to THR_MIN and flown at the same airspeed as used 
+ * to measure FW_T_CLMB_MAX.
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f);
 
-
-PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f);
-
-
-PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f);
-
-
-PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f);
-
-
-PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
-
-
-PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f);
-
-
-PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
-
-
-PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f);
-
-
-PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);
-
-
-PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
-
-
+/**
+ * Maximum descent rate
+ *
+ * This sets the maximum descent rate that the controller will use. 
+ * If this value is too large, the aircraft can over-speed on descent. 
+ * This should be set to a value that can be achieved without 
+ * exceeding the lower pitch angle limit and without over-speeding 
+ * the aircraft.
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
 
+/**
+ * TECS time constant
+ *
+ * This is the time constant of the TECS control algorithm (in seconds). 
+ * Smaller values make it faster to respond, larger values make it slower
+ * to respond.
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f);
+
+/**
+ * Throttle damping factor
+ *
+ * This is the damping gain for the throttle demand loop. 
+ * Increase to add damping to correct for oscillations in speed and height.
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f);
+
+/**
+ * Integrator gain
+ *
+ * This is the integrator gain on the control loop. 
+ * Increasing this gain increases the speed at which speed 
+ * and height offsets are trimmed out, but reduces damping and 
+ * increases overshoot.
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f);
+
+/**
+ * Maximum vertical acceleration
+ *
+ * This is the maximum vertical acceleration (in metres/second^2) 
+ * either up or down that the controller will use to correct speed 
+ * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) 
+ * allows for reasonably aggressive pitch changes if required to recover 
+ * from under-speed conditions.
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
+
+/**
+ * Complementary filter "omega" parameter for height
+ *
+ * This is the cross-over frequency (in radians/second) of the complementary 
+ * filter used to fuse vertical acceleration and barometric height to obtain 
+ * an estimate of height rate and height. Increasing this frequency weights 
+ * the solution more towards use of the barometer, whilst reducing it weights 
+ * the solution more towards use of the accelerometer data.
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f);
+
+/**
+ * Complementary filter "omega" parameter for speed
+ *
+ * This is the cross-over frequency (in radians/second) of the complementary 
+ * filter used to fuse longitudinal acceleration and airspeed to obtain an 
+ * improved airspeed estimate. Increasing this frequency weights the solution
+ * more towards use of the arispeed sensor, whilst reducing it weights the 
+ * solution more towards use of the accelerometer data.
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
+
+/**
+ * Roll -> Throttle feedforward
+ *
+ * Increasing this gain turn increases the amount of throttle that will 
+ * be used to compensate for the additional drag created by turning. 
+ * Ideally this should be set to  approximately 10 x the extra sink rate 
+ * in m/s created by a 45 degree bank turn. Increase this gain if 
+ * the aircraft initially loses energy in turns and reduce if the 
+ * aircraft initially gains energy in turns. Efficient high aspect-ratio 
+ * aircraft (eg powered sailplanes) can use a lower value, whereas 
+ * inefficient low aspect-ratio models (eg delta wings) can use a higher value.
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f);
+
+/**
+ * Speed <--> Altitude priority
+ *
+ * This parameter adjusts the amount of weighting that the pitch control 
+ * applies to speed vs height errors. Setting it to 0.0 will cause the 
+ * pitch control to control height and ignore speed errors. This will 
+ * normally improve height accuracy but give larger airspeed errors. 
+ * Setting it to 2.0 will cause the pitch control loop to control speed 
+ * and ignore height errors. This will normally reduce airspeed errors, 
+ * but give larger height errors. The default value of 1.0 allows the pitch 
+ * control to simultaneously control height and speed. 
+ * Note to Glider Pilots - set this parameter to 2.0 (The glider will 
+ * adjust its pitch angle to maintain airspeed, ignoring changes in height).
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);
+
+/**
+ * Pitch damping factor
+ *
+ * This is the damping gain for the pitch demand loop. Increase to add 
+ * damping to correct for oscillations in height. The default value of 0.0 
+ * will work well provided the pitch to servo controller has been tuned 
+ * properly.
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
+
+/**
+ * Height rate P factor
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f);
+
+/**
+ * Speed rate P factor
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f);
 
+/**
+ * Landing slope angle
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f);
-PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f);
+
+/**
+ *
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f);
+
+/**
+ * Landing flare altitude (relative)
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f);
+
+/**
+ * Landing throttle limit altitude (relative)
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f);
+
+/**
+ * Landing heading hold horizontal distance
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f);

src/modules/mavlink/mavlink.c

@@ -76,8 +76,20 @@
 #include <uORB/topics/mission_result.h>
 
 /* define MAVLink specific parameters */
+/**
+ * MAVLink system ID
+ * @group MAVLink
+ */
 PARAM_DEFINE_INT32(MAV_SYS_ID, 1);
+/**
+ * MAVLink component ID
+ * @group MAVLink
+ */
 PARAM_DEFINE_INT32(MAV_COMP_ID, 50);
+/**
+ * MAVLink type
+ * @group MAVLink
+ */
 PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING);
 
 __EXPORT int mavlink_main(int argc, char *argv[]);

src/modules/mavlink/mavlink_receiver.cpp

@@ -351,7 +351,7 @@ handle_message(mavlink_message_t *msg)
 		tstatus.rxerrors = rstatus.rxerrors;
 		tstatus.fixed = rstatus.fixed;
 
-		if (telemetry_status_pub == 0) {
+		if (telemetry_status_pub <= 0) {
 			telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
 
 		} else {

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -53,11 +53,9 @@
 #include <stdlib.h>
 #include <string.h>
 #include <unistd.h>
-#include <fcntl.h>
 #include <errno.h>
 #include <math.h>
 #include <poll.h>
-#include <time.h>
 #include <drivers/drv_hrt.h>
 #include <arch/board/board.h>
 #include <uORB/uORB.h>
@@ -71,7 +69,6 @@
 #include <uORB/topics/parameter_update.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
-#include <systemlib/pid/pid.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
 #include <mathlib/mathlib.h>
@@ -84,9 +81,9 @@
  */
 extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]);
 
-#define MIN_TAKEOFF_THROTTLE	0.3f
 #define YAW_DEADZONE	0.05f
-#define RATES_I_LIMIT	0.5f
+#define MIN_TAKEOFF_THRUST    0.2f
+#define RATES_I_LIMIT	0.3f
 
 class MulticopterAttitudeControl
 {
@@ -135,15 +132,13 @@ private:
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
-	math::Matrix<3, 3>	_R_sp;			/**< attitude setpoint rotation matrix */
-	math::Matrix<3, 3>	_R;				/**< rotation matrix for current state */
 	math::Vector<3>		_rates_prev;	/**< angular rates on previous step */
 	math::Vector<3>		_rates_sp;		/**< angular rates setpoint */
 	math::Vector<3>		_rates_int;		/**< angular rates integral error */
 	float				_thrust_sp;		/**< thrust setpoint */
 	math::Vector<3>		_att_control;	/**< attitude control vector */
 
-	math::Matrix<3, 3>	I;				/**< identity matrix */
+	math::Matrix<3, 3>  _I;				/**< identity matrix */
 
 	bool	_reset_yaw_sp;			/**< reset yaw setpoint flag */
 
@@ -262,13 +257,15 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_actuators_0_pub(-1),
 
 /* performance counters */
-	_loop_perf(perf_alloc(PC_ELAPSED, "fw att control"))
+	_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
 
 {
 	memset(&_v_att, 0, sizeof(_v_att));
 	memset(&_v_att_sp, 0, sizeof(_v_att_sp));
+	memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
 	memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
 	memset(&_v_control_mode, 0, sizeof(_v_control_mode));
+	memset(&_actuators, 0, sizeof(_actuators));
 	memset(&_armed, 0, sizeof(_armed));
 
 	_params.att_p.zero();
@@ -276,15 +273,13 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params.rate_i.zero();
 	_params.rate_d.zero();
 
-	_R_sp.identity();
-	_R.identity();
 	_rates_prev.zero();
 	_rates_sp.zero();
 	_rates_int.zero();
 	_thrust_sp = 0.0f;
 	_att_control.zero();
 
-	I.identity();
+	_I.identity();
 
 	_params_handles.roll_p			= 	param_find("MC_ROLL_P");
 	_params_handles.roll_rate_p		= 	param_find("MC_ROLLRATE_P");
@@ -535,16 +530,18 @@ MulticopterAttitudeControl::control_attitude(float dt)
 	_thrust_sp = _v_att_sp.thrust;
 
 	/* construct attitude setpoint rotation matrix */
+	math::Matrix<3, 3> R_sp;
+
 	if (_v_att_sp.R_valid) {
 		/* rotation matrix in _att_sp is valid, use it */
-		_R_sp.set(&_v_att_sp.R_body[0][0]);
+		R_sp.set(&_v_att_sp.R_body[0][0]);
 
 	} else {
 		/* rotation matrix in _att_sp is not valid, use euler angles instead */
-		_R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body);
+		R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body);
 
 		/* copy rotation matrix back to setpoint struct */
-		memcpy(&_v_att_sp.R_body[0][0], &_R_sp.data[0][0], sizeof(_v_att_sp.R_body));
+		memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0], sizeof(_v_att_sp.R_body));
 		_v_att_sp.R_valid = true;
 	}
 
@@ -561,23 +558,24 @@ MulticopterAttitudeControl::control_attitude(float dt)
 	}
 
 	/* rotation matrix for current state */
-	_R.set(_v_att.R);
+	math::Matrix<3, 3> R;
+	R.set(_v_att.R);
 
 	/* all input data is ready, run controller itself */
 
 	/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
-	math::Vector<3> R_z(_R(0, 2), _R(1, 2), _R(2, 2));
-	math::Vector<3> R_sp_z(_R_sp(0, 2), _R_sp(1, 2), _R_sp(2, 2));
+	math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2));
+	math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
 
 	/* axis and sin(angle) of desired rotation */
-	math::Vector<3> e_R = _R.transposed() * (R_z % R_sp_z);
+	math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);
 
 	/* calculate angle error */
 	float e_R_z_sin = e_R.length();
 	float e_R_z_cos = R_z * R_sp_z;
 
 	/* calculate weight for yaw control */
-	float yaw_w = _R_sp(2, 2) * _R_sp(2, 2);
+	float yaw_w = R_sp(2, 2) * R_sp(2, 2);
 
 	/* calculate rotation matrix after roll/pitch only rotation */
 	math::Matrix<3, 3> R_rp;
@@ -600,15 +598,15 @@ MulticopterAttitudeControl::control_attitude(float dt)
 		e_R_cp(2, 1) = e_R_z_axis(0);
 
 		/* rotation matrix for roll/pitch only rotation */
-		R_rp = _R * (I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
+		R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
 
 	} else {
 		/* zero roll/pitch rotation */
-		R_rp = _R;
+		R_rp = R;
 	}
 
-	/* R_rp and _R_sp has the same Z axis, calculate yaw error */
-	math::Vector<3> R_sp_x(_R_sp(0, 0), _R_sp(1, 0), _R_sp(2, 0));
+	/* R_rp and R_sp has the same Z axis, calculate yaw error */
+	math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
 	math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
 	e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;
 
@@ -616,7 +614,7 @@ MulticopterAttitudeControl::control_attitude(float dt)
 		/* for large thrust vector rotations use another rotation method:
 		 * calculate angle and axis for R -> R_sp rotation directly */
 		math::Quaternion q;
-		q.from_dcm(_R.transposed() * _R_sp);
+		q.from_dcm(R.transposed() * R_sp);
 		math::Vector<3> e_R_d = q.imag();
 		e_R_d.normalize();
 		e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));
@@ -658,7 +656,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
 	_rates_prev = rates;
 
 	/* update integral only if not saturated on low limit */
-	if (_thrust_sp > 0.1f) {
+	if (_thrust_sp > MIN_TAKEOFF_THRUST) {
 		for (int i = 0; i < 3; i++) {
 			if (fabsf(_att_control(i)) < _thrust_sp) {
 				float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
@@ -695,9 +693,6 @@ MulticopterAttitudeControl::task_main()
 	_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
 
-	/* rate limit attitude updates to 200Hz, failsafe against spam, normally runs at the same rate as attitude estimator */
-	orb_set_interval(_v_att_sub, 5);
-
 	/* initialize parameters cache */
 	parameters_update();
 
@@ -767,10 +762,12 @@ MulticopterAttitudeControl::task_main()
 				}
 
 			} else {
-				/* attitude controller disabled */
-				// TODO poll 'attitude_rates_setpoint' topic
-				_rates_sp.zero();
-				_thrust_sp = 0.0f;
+				/* attitude controller disabled, poll rates setpoint topic */
+				vehicle_rates_setpoint_poll();
+				_rates_sp(0) = _v_rates_sp.roll;
+				_rates_sp(1) = _v_rates_sp.pitch;
+				_rates_sp(2) = _v_rates_sp.yaw;
+				_thrust_sp = _v_rates_sp.thrust;
 			}
 
 			if (_v_control_mode.flag_control_rates_enabled) {

src/modules/mc_att_control/mc_att_control_params.c

@@ -41,16 +41,135 @@
 
 #include <systemlib/param/param.h>
 
+/**
+ * Roll P gain
+ *
+ * Roll proportional gain, i.e. desired angular speed in rad/s for error 1 rad.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
 PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f);
+
+/**
+ * Roll rate P gain
+ *
+ * Roll rate proportional gain, i.e. control output for angular speed error 1 rad/s.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
 PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f);
+
+/**
+ * Roll rate I gain
+ *
+ * Roll rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
 PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f);
+
+/**
+ * Roll rate D gain
+ *
+ * Roll rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
 PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f);
+
+/**
+ * Pitch P gain
+ *
+ * Pitch proportional gain, i.e. desired angular speed in rad/s for error 1 rad.
+ *
+ * @unit 1/s
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
 PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f);
+
+/**
+ * Pitch rate P gain
+ *
+ * Pitch rate proportional gain, i.e. control output for angular speed error 1 rad/s.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
 PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f);
+
+/**
+ * Pitch rate I gain
+ *
+ * Pitch rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
 PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f);
+
+/**
+ * Pitch rate D gain
+ *
+ * Pitch rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
 PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f);
+
+/**
+ * Yaw P gain
+ *
+ * Yaw proportional gain, i.e. desired angular speed in rad/s for error 1 rad.
+ *
+ * @unit 1/s
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
 PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f);
+
+/**
+ * Yaw rate P gain
+ *
+ * Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
 PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f);
+
+/**
+ * Yaw rate I gain
+ *
+ * Yaw rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
 PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f);
+
+/**
+ * Yaw rate D gain
+ *
+ * Yaw rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
 PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
+
+/**
+ * Yaw feed forward
+ *
+ * Feed forward weight for manual yaw control. 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Multicopter Attitude Control
+ */
 PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f);

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -51,7 +51,6 @@
 #include <errno.h>
 #include <math.h>
 #include <poll.h>
-#include <time.h>
 #include <drivers/drv_hrt.h>
 #include <arch/board/board.h>
 #include <uORB/uORB.h>
@@ -68,7 +67,6 @@
 #include <uORB/topics/vehicle_global_velocity_setpoint.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
-#include <systemlib/pid/pid.h>
 #include <systemlib/systemlib.h>
 #include <mathlib/mathlib.h>
 #include <lib/geo/geo.h>
@@ -732,7 +730,6 @@ MulticopterPositionControl::task_main()
 			} else {
 				/* run position & altitude controllers, calculate velocity setpoint */
 				math::Vector<3> pos_err;
-				float err_x, err_y;
 				get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_err.data[0], &pos_err.data[1]);
 				pos_err(2) = -(_alt_sp - alt);
 
@@ -794,7 +791,6 @@ MulticopterPositionControl::task_main()
 							}
 
 							thrust_int(2) = -i;
-							mavlink_log_info(_mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i);
 						}
 
 					} else {
@@ -806,7 +802,6 @@ MulticopterPositionControl::task_main()
 							reset_int_xy = false;
 							thrust_int(0) = 0.0f;
 							thrust_int(1) = 0.0f;
-							mavlink_log_info(_mavlink_fd, "[mpc] reset xy vel integral");
 						}
 
 					} else {

src/modules/mc_pos_control/mc_pos_control_params.c

@@ -39,20 +39,164 @@
 
 #include <systemlib/param/param.h>
 
-PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.0f);
+/**
+ * Minimum thrust
+ *
+ * Minimum vertical thrust. It's recommended to set it > 0 to avoid free fall with zero thrust.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.1f);
+
+/**
+ * Maximum thrust
+ *
+ * Limit max allowed thrust.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Multicopter Position Control
+ */
 PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.0f);
+
+/**
+ * Proportional gain for vertical position error
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
 PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f);
+
+/**
+ * Proportional gain for vertical velocity error
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
 PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f);
+
+/**
+ * Integral gain for vertical velocity error
+ *
+ * Non zero value allows hovering thrust estimation on stabilized or autonomous takeoff.
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
 PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.02f);
+
+/**
+ * Differential gain for vertical velocity error
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
 PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f);
+
+/**
+ * Maximum vertical velocity
+ *
+ * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (SEATBELT, EASY).
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
 PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f);
+
+/**
+ * Vertical velocity feed forward
+ *
+ * Feed forward weight for altitude control in stabilized modes (SEATBELT, EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Multicopter Position Control
+ */
 PARAM_DEFINE_FLOAT(MPC_Z_FF, 0.5f);
+
+/**
+ * Proportional gain for horizontal position error
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
 PARAM_DEFINE_FLOAT(MPC_XY_P, 1.0f);
+
+/**
+ * Proportional gain for horizontal velocity error
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
 PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.1f);
+
+/**
+ * Integral gain for horizontal velocity error
+ *
+ * Non-zero value allows to resist wind.
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
 PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.02f);
+
+/**
+ * Differential gain for horizontal velocity error. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
 PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f);
+
+/**
+ * Maximum horizontal velocity
+ *
+ * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (EASY).
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
 PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f);
+
+/**
+ * Horizontal velocity feed forward
+ *
+ * Feed forward weight for position control in position control mode (EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Multicopter Position Control
+ */
 PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f);
+
+/**
+ * Maximum tilt
+ *
+ * Limits maximum tilt in AUTO and EASY modes.
+ *
+ * @min 0.0
+ * @max 1.57
+ * @group Multicopter Position Control
+ */
 PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 1.0f);
+
+/**
+ * Landing descend rate
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
 PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f);
+
+/**
+ * Maximum landing tilt
+ *
+ * Limits maximum tilt on landing.
+ *
+ * @min 0.0
+ * @max 1.57
+ * @group Multicopter Position Control
+ */
 PARAM_DEFINE_FLOAT(MPC_LAND_TILT, 0.3f);

src/modules/navigator/geofence_params.c

@@ -45,11 +45,17 @@
 #include <systemlib/param/param.h>
 
 /*
- * geofence parameters, accessible via MAVLink
- *
+ * Geofence parameters, accessible via MAVLink
  */
 
-// @DisplayName		Switch to enable geofence
-// @Description		if set to 1 geofence is enabled, defaults to 1 because geofence is only enabled when the geofence.txt file is present
-// @Range		0 or 1
+/**
+ * Enable geofence.
+ *
+ * Set to 1 to enable geofence.
+ * Defaults to 1 because geofence is only enabled when the geofence.txt file is present.
+ *
+ * @min 0
+ * @max 1
+ * @group Geofence
+ */
 PARAM_DEFINE_INT32(GF_ON, 1);

src/modules/navigator/navigator_main.cpp

@@ -154,17 +154,16 @@ private:
 	int		_capabilities_sub;		/**< notification of vehicle capabilities updates */
 	int		_control_mode_sub;		/**< vehicle control mode subscription */
 
-	orb_advert_t	_pos_sp_triplet_pub;			/**< publish position setpoint triplet */
+	orb_advert_t	_pos_sp_triplet_pub;		/**< publish position setpoint triplet */
 	orb_advert_t	_mission_result_pub;		/**< publish mission result topic */
 
 	struct vehicle_status_s				_vstatus;		/**< vehicle status */
-	struct vehicle_control_mode_s		_control_mode;		/**< vehicle control mode */
+	struct vehicle_control_mode_s			_control_mode;		/**< vehicle control mode */
 	struct vehicle_global_position_s		_global_pos;		/**< global vehicle position */
 	struct home_position_s				_home_pos;		/**< home position for RTL */
-	struct position_setpoint_triplet_s			_pos_sp_triplet;	/**< triplet of position setpoints */
+	struct position_setpoint_triplet_s		_pos_sp_triplet;	/**< triplet of position setpoints */
 	struct mission_result_s				_mission_result;	/**< mission result for commander/mavlink */
-	struct mission_item_s				_mission_item;	/**< current mission item */
-	bool		_mission_item_valid;	/**< current mission item valid */
+	struct mission_item_s				_mission_item;		/**< current mission item */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
@@ -178,21 +177,22 @@ private:
 
 	class 		Mission				_mission;
 
-	bool		_global_pos_valid;				/**< track changes of global_position.global_valid flag */
-	bool		_reset_loiter_pos;				/**< if true then loiter position should be set to current position */
+	bool		_mission_item_valid;		/**< current mission item valid */
+	bool		_global_pos_valid;		/**< track changes of global_position.global_valid flag */
+	bool		_reset_loiter_pos;		/**< if true then loiter position should be set to current position */
 	bool		_waypoint_position_reached;
 	bool		_waypoint_yaw_reached;
 	uint64_t	_time_first_inside_orbit;
-	bool		_need_takeoff;		/**< if need to perform vertical takeoff before going to waypoint (only for MISSION mode and VTOL vehicles) */
-	bool		_do_takeoff;		/**< vertical takeoff state, current mission item is generated by navigator (only for MISSION mode and VTOL vehicles) */
+	bool		_need_takeoff;			/**< if need to perform vertical takeoff before going to waypoint (only for MISSION mode and VTOL vehicles) */
+	bool		_do_takeoff;			/**< vertical takeoff state, current mission item is generated by navigator (only for MISSION mode and VTOL vehicles) */
 
 	MissionFeasibilityChecker missionFeasiblityChecker;
 
-	uint64_t	_set_nav_state_timestamp;		/**< timestamp of last handled navigation state request */
+	uint64_t	_set_nav_state_timestamp;	/**< timestamp of last handled navigation state request */
 
 	bool		_pos_sp_triplet_updated;
 
-	char *nav_states_str[NAV_STATE_MAX];
+	const char *nav_states_str[NAV_STATE_MAX];
 
 	struct {
 		float min_altitude;
@@ -305,6 +305,12 @@ private:
 	void		start_land();
 	void		start_land_home();
 
+	/**
+	 * Fork for state transitions
+	 */
+	void		request_loiter_or_ready();
+	void		request_mission_if_available();
+
 	/**
 	 * Guards offboard mission
 	 */
@@ -315,6 +321,11 @@ private:
 	 */
 	bool		onboard_mission_available(unsigned relative_index);
 
+	/**
+	 * Reset all "reached" flags.
+	 */
+	void		reset_reached();
+
 	/**
 	 * Check if current mission item has been reached.
 	 */
@@ -376,11 +387,11 @@ Navigator::Navigator() :
 	_global_pos_sub(-1),
 	_home_pos_sub(-1),
 	_vstatus_sub(-1),
-	_control_mode_sub(-1),
 	_params_sub(-1),
 	_offboard_mission_sub(-1),
 	_onboard_mission_sub(-1),
 	_capabilities_sub(-1),
+	_control_mode_sub(-1),
 
 /* publications */
 	_pos_sp_triplet_pub(-1),
@@ -389,22 +400,22 @@ Navigator::Navigator() :
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
 
-/* states */
-	_rtl_state(RTL_STATE_NONE),
+	_geofence_violation_warning_sent(false),
 	_fence_valid(false),
 	_inside_fence(true),
 	_mission(),
+	_mission_item_valid(false),
 	_global_pos_valid(false),
 	_reset_loiter_pos(true),
 	_waypoint_position_reached(false),
 	_waypoint_yaw_reached(false),
 	_time_first_inside_orbit(0),
-	_set_nav_state_timestamp(0),
-	_mission_item_valid(false),
 	_need_takeoff(true),
 	_do_takeoff(false),
+	_set_nav_state_timestamp(0),
 	_pos_sp_triplet_updated(false),
-	_geofence_violation_warning_sent(false)
+/* states */
+	_rtl_state(RTL_STATE_NONE)
 {
 	_parameter_handles.min_altitude = param_find("NAV_MIN_ALT");
 	_parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD");
@@ -689,7 +700,7 @@ Navigator::task_main()
 					if (_vstatus.return_switch == RETURN_SWITCH_RETURN) {
 						/* switch to RTL if not already landed after RTL and home position set */
 						if (!(_rtl_state == RTL_STATE_DESCEND &&
-						      (myState == NAV_STATE_READY || myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
+						      (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
 						    _vstatus.condition_home_position_valid) {
 							dispatch(EVENT_RTL_REQUESTED);
 						}
@@ -699,24 +710,17 @@ Navigator::task_main()
 					} else {
 						/* MISSION switch */
 						if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) {
-							dispatch(EVENT_LOITER_REQUESTED);
+							request_loiter_or_ready();
 							stick_mode = true;
 
 						} else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) {
-							/* switch to mission only if available */
-							if (_mission.current_mission_available()) {
-								dispatch(EVENT_MISSION_REQUESTED);
-
-							} else {
-								dispatch(EVENT_LOITER_REQUESTED);
-							}
-
+							request_mission_if_available();
 							stick_mode = true;
 						}
 
 						if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) {
 							/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
-							dispatch(EVENT_LOITER_REQUESTED);
+							request_mission_if_available();
 							stick_mode = true;
 						}
 					}
@@ -733,22 +737,16 @@ Navigator::task_main()
 							break;
 
 						case NAV_STATE_LOITER:
-							dispatch(EVENT_LOITER_REQUESTED);
+							request_loiter_or_ready();
 							break;
 
 						case NAV_STATE_MISSION:
-							if (_mission.current_mission_available()) {
-								dispatch(EVENT_MISSION_REQUESTED);
-
-							} else {
-								dispatch(EVENT_LOITER_REQUESTED);
-							}
-
+							request_mission_if_available();
 							break;
 
 						case NAV_STATE_RTL:
 							if (!(_rtl_state == RTL_STATE_DESCEND &&
-							      (myState == NAV_STATE_READY || myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
+							      (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
 							    _vstatus.condition_home_position_valid) {
 								dispatch(EVENT_RTL_REQUESTED);
 							}
@@ -756,9 +754,7 @@ Navigator::task_main()
 							break;
 
 						case NAV_STATE_LAND:
-							if (myState != NAV_STATE_READY) {
-								dispatch(EVENT_LAND_REQUESTED);
-							}
+							dispatch(EVENT_LAND_REQUESTED);
 
 							break;
 
@@ -770,12 +766,7 @@ Navigator::task_main()
 					} else {
 						/* on first switch to AUTO try mission by default, if none is available fallback to loiter */
 						if (myState == NAV_STATE_NONE) {
-							if (_mission.current_mission_available()) {
-								dispatch(EVENT_MISSION_REQUESTED);
-
-							} else {
-								dispatch(EVENT_LOITER_REQUESTED);
-							}
+							request_mission_if_available();
 						}
 					}
 				}
@@ -865,11 +856,8 @@ Navigator::task_main()
 
 		/* notify user about state changes */
 		if (myState != prevState) {
-			mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s", nav_states_str[myState]);
+			mavlink_log_info(_mavlink_fd, "#audio: navigation state: %s", nav_states_str[myState]);
 			prevState = myState;
-
-			/* reset time counter on state changes */
-			_time_first_inside_orbit = 0;
 		}
 
 		perf_end(_loop_perf);
@@ -967,7 +955,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
 		/* EVENT_READY_REQUESTED */		{NO_ACTION, NAV_STATE_READY},
 		/* EVENT_LOITER_REQUESTED */		{NO_ACTION, NAV_STATE_READY},
 		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
-		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
+		/* EVENT_RTL_REQUESTED */		{NO_ACTION, NAV_STATE_READY},
 		/* EVENT_LAND_REQUESTED */		{NO_ACTION, NAV_STATE_READY},
 		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_READY},
 		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_READY},
@@ -1021,6 +1009,8 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
 void
 Navigator::start_none()
 {
+	reset_reached();
+
 	_pos_sp_triplet.previous.valid = false;
 	_pos_sp_triplet.current.valid = false;
 	_pos_sp_triplet.next.valid = false;
@@ -1036,6 +1026,8 @@ Navigator::start_none()
 void
 Navigator::start_ready()
 {
+	reset_reached();
+
 	_pos_sp_triplet.previous.valid = false;
 	_pos_sp_triplet.current.valid = true;
 	_pos_sp_triplet.next.valid = false;
@@ -1058,6 +1050,8 @@ Navigator::start_ready()
 void
 Navigator::start_loiter()
 {
+	reset_reached();
+
 	_do_takeoff = false;
 
 	/* set loiter position if needed */
@@ -1071,18 +1065,17 @@ Navigator::start_loiter()
 		float min_alt_amsl = _parameters.min_altitude + _home_pos.alt;
 
 		/* use current altitude if above min altitude set by parameter */
-		if (_global_pos.alt < min_alt_amsl) {
+		if (_global_pos.alt < min_alt_amsl && !_vstatus.is_rotary_wing) {
 			_pos_sp_triplet.current.alt = min_alt_amsl;
-			mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt));
+			mavlink_log_info(_mavlink_fd, "#audio: loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt));
 
 		} else {
 			_pos_sp_triplet.current.alt = _global_pos.alt;
-			mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude");
+			mavlink_log_info(_mavlink_fd, "#audio: loiter at current altitude");
 		}
 
-		_pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER;
 	}
-
+	_pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER;
 	_pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius;
 	_pos_sp_triplet.current.loiter_direction = 1;
 	_pos_sp_triplet.previous.valid = false;
@@ -1104,6 +1097,8 @@ Navigator::start_mission()
 void
 Navigator::set_mission_item()
 {
+	reset_reached();
+
 	/* copy current mission to previous item */
 	memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
 
@@ -1117,9 +1112,6 @@ Navigator::set_mission_item()
 	ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index);
 
 	if (ret == OK) {
-		/* reset time counter for new item */
-		_time_first_inside_orbit = 0;
-
 		_mission_item_valid = true;
 		position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
 
@@ -1175,14 +1167,14 @@ Navigator::set_mission_item()
 		}
 
 		if (_do_takeoff) {
-			mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm above home", _pos_sp_triplet.current.alt - _home_pos.alt);
+			mavlink_log_info(_mavlink_fd, "#audio: takeoff to %.1fm above home", (double)(_pos_sp_triplet.current.alt - _home_pos.alt));
 
 		} else {
 			if (onboard) {
-				mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index);
+				mavlink_log_info(_mavlink_fd, "#audio: heading to onboard WP %d", index);
 
 			} else {
-				mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index);
+				mavlink_log_info(_mavlink_fd, "#audio: heading to offboard WP %d", index);
 			}
 		}
 
@@ -1237,6 +1229,8 @@ Navigator::start_rtl()
 void
 Navigator::start_land()
 {
+	reset_reached();
+
 	/* this state can be requested by commander even if no global position available,
 	 * in his case controller must perform landing without position control */
 	_do_takeoff = false;
@@ -1268,6 +1262,8 @@ Navigator::start_land()
 void
 Navigator::start_land_home()
 {
+	reset_reached();
+
 	/* land to home position, should be called when hovering above home, from RTL state */
 	_do_takeoff = false;
 	_reset_loiter_pos = true;
@@ -1298,8 +1294,7 @@ Navigator::start_land_home()
 void
 Navigator::set_rtl_item()
 {
-	/*reset time counter for new RTL item */
-	_time_first_inside_orbit = 0;
+	reset_reached();
 
 	switch (_rtl_state) {
 	case RTL_STATE_CLIMB: {
@@ -1331,7 +1326,7 @@ Navigator::set_rtl_item()
 
 			_pos_sp_triplet.next.valid = false;
 
-			mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm above home", climb_alt - _home_pos.alt);
+			mavlink_log_info(_mavlink_fd, "#audio: RTL: climb to %.1fm above home", (double)(climb_alt - _home_pos.alt));
 			break;
 		}
 
@@ -1343,7 +1338,14 @@ Navigator::set_rtl_item()
 			_mission_item.lat = _home_pos.lat;
 			_mission_item.lon = _home_pos.lon;
 			// don't change altitude
-			_mission_item.yaw = NAN;	// TODO set heading to home
+			if (_pos_sp_triplet.previous.valid) {
+				/* if previous setpoint is valid then use it to calculate heading to home */
+				_mission_item.yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, _mission_item.lat, _mission_item.lon);
+
+			} else {
+				/* else use current position */
+				_mission_item.yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, _mission_item.lat, _mission_item.lon);
+			}
 			_mission_item.loiter_radius = _parameters.loiter_radius;
 			_mission_item.loiter_direction = 1;
 			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
@@ -1357,7 +1359,7 @@ Navigator::set_rtl_item()
 
 			_pos_sp_triplet.next.valid = false;
 
-			mavlink_log_info(_mavlink_fd, "[navigator] RTL: return at %.1fm above home", _mission_item.altitude - _home_pos.alt);
+			mavlink_log_info(_mavlink_fd, "#audio: RTL: return at %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt));
 			break;
 		}
 
@@ -1375,7 +1377,7 @@ Navigator::set_rtl_item()
 			_mission_item.loiter_direction = 1;
 			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
 			_mission_item.acceptance_radius = _parameters.acceptance_radius;
-			_mission_item.time_inside = _parameters.rtl_land_delay < 0.0 ? 0.0f : _parameters.rtl_land_delay;
+			_mission_item.time_inside = _parameters.rtl_land_delay < 0.0f ? 0.0f : _parameters.rtl_land_delay;
 			_mission_item.pitch_min = 0.0f;
 			_mission_item.autocontinue = _parameters.rtl_land_delay > -0.001f;
 			_mission_item.origin = ORIGIN_ONBOARD;
@@ -1384,12 +1386,12 @@ Navigator::set_rtl_item()
 
 			_pos_sp_triplet.next.valid = false;
 
-			mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm above home", _mission_item.altitude - _home_pos.alt);
+			mavlink_log_info(_mavlink_fd, "#audio: RTL: descend to %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt));
 			break;
 		}
 
 	default: {
-			mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state);
+			mavlink_log_critical(_mavlink_fd, "#audio: [navigator] error: unknown RTL state: %d", _rtl_state);
 			start_loiter();
 			break;
 		}
@@ -1398,6 +1400,29 @@ Navigator::set_rtl_item()
 	_pos_sp_triplet_updated = true;
 }
 
+void
+Navigator::request_loiter_or_ready()
+{
+	/* XXX workaround: no landing detector for fixedwing yet */
+	if (_vstatus.condition_landed && _vstatus.is_rotary_wing) {
+		dispatch(EVENT_READY_REQUESTED);
+
+	} else {
+		dispatch(EVENT_LOITER_REQUESTED);
+	}
+}
+
+void
+Navigator::request_mission_if_available()
+{
+	if (_mission.current_mission_available()) {
+		dispatch(EVENT_MISSION_REQUESTED);
+
+	} else {
+		request_loiter_or_ready();
+	}
+}
+
 void
 Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item)
 {
@@ -1409,17 +1434,28 @@ Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_
 		sp->lon = _home_pos.lon;
 		sp->alt = _home_pos.alt + _parameters.rtl_alt;
 
+		if (_pos_sp_triplet.previous.valid) {
+			/* if previous setpoint is valid then use it to calculate heading to home */
+			sp->yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, sp->lat, sp->lon);
+
+		} else {
+			/* else use current position */
+			sp->yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, sp->lat, sp->lon);
+		}
+		sp->loiter_radius = _parameters.loiter_radius;
+		sp->loiter_direction = 1;
+		sp->pitch_min = 0.0f;
+
 	} else {
 		sp->lat = item->lat;
 		sp->lon = item->lon;
 		sp->alt = item->altitude_is_relative ? item->altitude + _home_pos.alt : item->altitude;
+		sp->yaw = item->yaw;
+		sp->loiter_radius = item->loiter_radius;
+		sp->loiter_direction = item->loiter_direction;
+		sp->pitch_min = item->pitch_min;
 	}
 
-	sp->yaw = item->yaw;
-	sp->loiter_radius = item->loiter_radius;
-	sp->loiter_direction = item->loiter_direction;
-	sp->pitch_min = item->pitch_min;
-
 	if (item->nav_cmd == NAV_CMD_TAKEOFF) {
 		sp->type = SETPOINT_TYPE_TAKEOFF;
 
@@ -1496,7 +1532,7 @@ Navigator::check_mission_item_reached()
 		}
 	}
 
-	if (!_waypoint_yaw_reached) {
+	if (_waypoint_position_reached && !_waypoint_yaw_reached) {
 		if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item.yaw)) {
 			/* check yaw if defined only for rotary wing except takeoff */
 			float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw);
@@ -1516,16 +1552,14 @@ Navigator::check_mission_item_reached()
 			_time_first_inside_orbit = now;
 
 			if (_mission_item.time_inside > 0.01f) {
-				mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item.time_inside);
+				mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs", (double)_mission_item.time_inside);
 			}
 		}
 
 		/* check if the MAV was long enough inside the waypoint orbit */
 		if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item.time_inside * 1e6)
 		    || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
-			_time_first_inside_orbit = 0;
-			_waypoint_yaw_reached = false;
-			_waypoint_position_reached = false;
+			reset_reached();
 			return true;
 		}
 	}
@@ -1534,6 +1568,15 @@ Navigator::check_mission_item_reached()
 
 }
 
+void
+Navigator::reset_reached()
+{
+	_time_first_inside_orbit = 0;
+	_waypoint_position_reached = false;
+	_waypoint_yaw_reached = false;
+
+}
+
 void
 Navigator::on_mission_item_reached()
 {
@@ -1541,7 +1584,7 @@ Navigator::on_mission_item_reached()
 		if (_do_takeoff) {
 			/* takeoff completed */
 			_do_takeoff = false;
-			mavlink_log_info(_mavlink_fd, "[navigator] takeoff completed");
+			mavlink_log_info(_mavlink_fd, "#audio: takeoff completed");
 
 		} else {
 			/* advance by one mission item */
@@ -1556,13 +1599,7 @@ Navigator::on_mission_item_reached()
 			/* loiter at last waypoint */
 			_reset_loiter_pos = false;
 			mavlink_log_info(_mavlink_fd, "[navigator] mission completed");
-
-			if (_vstatus.condition_landed) {
-				dispatch(EVENT_READY_REQUESTED);
-
-			} else {
-				dispatch(EVENT_LOITER_REQUESTED);
-			}
+			request_loiter_or_ready();
 		}
 
 	} else if (myState == NAV_STATE_RTL) {

src/modules/navigator/navigator_params.c

@@ -50,14 +50,91 @@
 
 /*
  * Navigator parameters, accessible via MAVLink
- *
  */
 
+/**
+ * Minimum altitude (fixed wing only)
+ *
+ * Minimum altitude above home for LOITER.
+ *
+ * @unit meters
+ * @group Navigation
+ */
 PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f);
+
+/**
+ * Waypoint acceptance radius
+ *
+ * Default value of acceptance radius (if not specified in mission item).
+ *
+ * @unit meters
+ * @min 0.0
+ * @group Navigation
+ */
 PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f);
+
+/**
+ * Loiter radius (fixed wing only)
+ *
+ * Default value of loiter radius (if not specified in mission item).
+ *
+ * @unit meters
+ * @min 0.0
+ * @group Navigation
+ */
 PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
+
+/**
+ * Enable onboard mission
+ *
+ * @group Navigation
+ */
 PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0);
-PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f);	// default TAKEOFF altitude
-PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f);		// slow descend from this altitude when landing
-PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f);		// min altitude for going home in RTL mode
-PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f);	// delay after descend before landing, if set to -1 the system will not land but loiter at NAV_LAND_ALT
+
+/**
+ * Take-off altitude
+ *
+ * Even if first waypoint has altitude less then NAV_TAKEOFF_ALT above home position, system will climb to NAV_TAKEOFF_ALT on takeoff, then go to waypoint.
+ *
+ * @unit meters
+ * @group Navigation
+ */
+PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f);
+
+/**
+ * Landing altitude
+ *
+ * Stay at this altitude above home position after RTL descending. Land (i.e. slowly descend) from this altitude if autolanding allowed.
+ *
+ * @unit meters
+ * @group Navigation
+ */
+PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f);
+
+/**
+ * Return-To-Launch altitude
+ *
+ * Minimum altitude above home position for going home in RTL mode.
+ *
+ * @unit meters
+ * @group Navigation
+ */
+PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f);
+
+/**
+ * Return-To-Launch delay
+ *
+ * Delay after descend before landing in RTL mode.
+ * If set to -1 the system will not land but loiter at NAV_LAND_ALT.
+ *
+ * @unit seconds
+ * @group Navigation
+ */
+PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f);
+
+/**
+ * Enable parachute deployment
+ *
+ * @group Navigation
+ */
+PARAM_DEFINE_INT32(NAV_PARACHUTE_EN, 0);

src/modules/position_estimator_inav/position_estimator_inav_main.c

@@ -42,14 +42,11 @@
 #include <stdio.h>
 #include <stdbool.h>
 #include <fcntl.h>
-#include <float.h>
 #include <string.h>
 #include <nuttx/config.h>
 #include <nuttx/sched.h>
 #include <sys/prctl.h>
 #include <termios.h>
-#include <errno.h>
-#include <limits.h>
 #include <math.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/parameter_update.h>
@@ -170,12 +167,13 @@ void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3],
 	FILE *f = fopen("/fs/microsd/inav.log", "a");
 	if (f) {
 		char *s = malloc(256);
-		snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2]);
-		fputs(f, s);
-		snprintf(s, 256, "\tacc_corr=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", corr_acc[0], corr_acc[1], corr_acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v);
-		fputs(f, s);
+		unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2]);
+		fwrite(s, 1, n, f);
+		n = snprintf(s, 256, "\tacc_corr=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", corr_acc[0], corr_acc[1], corr_acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v);
+		fwrite(s, 1, n, f);
 		free(s);
 	}
+	fsync(fileno(f));
 	fclose(f);
 }
 
@@ -711,6 +709,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
 		inertial_filter_correct(corr_acc[2], dt, z_est, 2, params.w_z_acc);
 
+		float x_est_prev[3], y_est_prev[3];
+
+		memcpy(x_est_prev, x_est, sizeof(x_est));
+		memcpy(y_est_prev, y_est, sizeof(y_est));
+
 		if (can_estimate_xy) {
 			/* inertial filter prediction for position */
 			inertial_filter_predict(dt, x_est);
@@ -718,7 +721,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 			if (!isfinite(x_est[0]) || !isfinite(y_est[0])) {
 				write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
-				thread_should_exit = true;
+				memcpy(x_est, x_est_prev, sizeof(x_est));
+				memcpy(y_est, y_est_prev, sizeof(y_est));
 			}
 
 			/* inertial filter correction for position */
@@ -742,7 +746,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 			if (!isfinite(x_est[0]) || !isfinite(y_est[0])) {
 				write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
-				thread_should_exit = true;
+				memcpy(x_est, x_est_prev, sizeof(x_est));
+				memcpy(y_est, y_est_prev, sizeof(y_est));
+				memset(corr_acc, 0, sizeof(corr_acc));
+				memset(corr_gps, 0, sizeof(corr_gps));
+				memset(corr_flow, 0, sizeof(corr_flow));
 			}
 		}
 

src/modules/px4iofirmware/dsm.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 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
@@ -162,6 +162,7 @@ dsm_guess_format(bool reset)
 		0xff,	/* 8 channels (DX8) */
 		0x1ff,	/* 9 channels (DX9, etc.) */
 		0x3ff,	/* 10 channels (DX10) */
+		0x1fff,	/* 13 channels (DX10t) */
 		0x3fff	/* 18 channels (DX10) */
 	};
 	unsigned votes10 = 0;
@@ -368,11 +369,25 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 		if (channel >= *num_values)
 			*num_values = channel + 1;
 
-		/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
-		if (dsm_channel_shift == 11)
-			value /= 2;
+		/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding. */
+		if (dsm_channel_shift == 10)
+			value *= 2;
 
-		value += 998;
+		/*
+		 * Spektrum scaling is special. There are these basic considerations
+		 *
+		 *   * Midpoint is 1520 us
+		 *   * 100% travel channels are +- 400 us
+		 *
+		 * We obey the original Spektrum scaling (so a default setup will scale from
+		 * 1100 - 1900 us), but we do not obey the weird 1520 us center point
+		 * and instead (correctly) center the center around 1500 us. This is in order
+		 * to get something useful without requiring the user to calibrate on a digital
+		 * link for no reason.
+		 */
+
+		/* scaled integer for decent accuracy while staying efficient */
+		value = ((((int)value - 1024) * 1000) / 1700) + 1500;
 
 		/*
 		 * Store the decoded channel into the R/C input buffer, taking into
@@ -400,6 +415,15 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 		values[channel] = value;
 	}
 
+	/*
+	 * Spektrum likes to send junk in higher channel numbers to fill
+	 * their packets. We don't know about a 13 channel model in their TX
+	 * lines, so if we get a channel count of 13, we'll return 12 (the last
+	 * data index that is stable).
+	 */
+	if (*num_values == 13)
+		*num_values = 12;
+
 	if (dsm_channel_shift == 11) {
 		/* Set the 11-bit data indicator */
 		*num_values |= 0x8000;

src/modules/px4iofirmware/mixer.cpp

@@ -179,7 +179,7 @@ mixer_tick(void)
 							    ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)
 		/* and there is valid input via or mixer */         &&   (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) )
 		/* or direct PWM is set */               || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)
-		/* or failsafe was set manually */	 || (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)
+		/* or failsafe was set manually */	 || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK))
 						     )
 	);
 

src/modules/px4iofirmware/registers.c

@@ -566,7 +566,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			break;
 
 		case PX4IO_P_SETUP_DSM:
-			dsm_bind(value & 0x0f, (value >> 4) & 7);
+			dsm_bind(value & 0x0f, (value >> 4) & 0xF);
 			break;
 
 		default:

src/modules/sdlog2/logbuffer.c

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: Anton Babushkin <anton.babushkin@me.com>
+ *   Copyright (c) 2013, 2014 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

src/modules/sdlog2/sdlog2.c

@@ -1,8 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
- *           Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -82,6 +80,7 @@
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/esc_status.h>
+#include <uORB/topics/telemetry_status.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
@@ -450,6 +449,7 @@ static void *logwriter_thread(void *arg)
 				n = available;
 			}
 
+			lseek(log_fd, 0, SEEK_CUR);
 			n = write(log_fd, read_ptr, n);
 
 			should_wait = (n == available) && !is_part;
@@ -758,6 +758,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct esc_status_s esc;
 		struct vehicle_global_velocity_setpoint_s global_vel_sp;
 		struct battery_status_s battery;
+		struct telemetry_status_s telemetry;
 	} buf;
 
 	memset(&buf, 0, sizeof(buf));
@@ -783,6 +784,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int esc_sub;
 		int global_vel_sp_sub;
 		int battery_sub;
+		int telemetry_sub;
 	} subs;
 
 	/* log message buffer: header + body */
@@ -811,6 +813,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_GVSP_s log_GVSP;
 			struct log_BATT_s log_BATT;
 			struct log_DIST_s log_DIST;
+			struct log_TELE_s log_TELE;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -946,6 +949,12 @@ int sdlog2_thread_main(int argc, char *argv[])
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
+	/* --- TELEMETRY STATUS --- */
+	subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
+	fds[fdsc_count].fd = subs.telemetry_sub;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;
+
 	/* WARNING: If you get the error message below,
 	 * then the number of registered messages (fdsc)
 	 * differs from the number of messages in the above list.
@@ -1347,6 +1356,20 @@ int sdlog2_thread_main(int argc, char *argv[])
 				LOGBUFFER_WRITE_AND_COUNT(BATT);
 			}
 
+			/* --- TELEMETRY --- */
+			if (fds[ifds++].revents & POLLIN) {
+				orb_copy(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry);
+				log_msg.msg_type = LOG_TELE_MSG;
+				log_msg.body.log_TELE.rssi = buf.telemetry.rssi;
+				log_msg.body.log_TELE.remote_rssi = buf.telemetry.remote_rssi;
+				log_msg.body.log_TELE.noise = buf.telemetry.noise;
+				log_msg.body.log_TELE.remote_noise = buf.telemetry.remote_noise;
+				log_msg.body.log_TELE.rxerrors = buf.telemetry.rxerrors;
+				log_msg.body.log_TELE.fixed = buf.telemetry.fixed;
+				log_msg.body.log_TELE.txbuf = buf.telemetry.txbuf;
+				LOGBUFFER_WRITE_AND_COUNT(TELE);
+			}
+
 			/* signal the other thread new data, but not yet unlock */
 			if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
 				/* only request write if several packets can be written at once */

src/modules/sdlog2/sdlog2_messages.h

@@ -264,6 +264,18 @@ struct log_DIST_s {
 	uint8_t flags;
 };
 
+/* --- TELE - TELEMETRY STATUS --- */
+#define LOG_TELE_MSG 22
+struct log_TELE_s {
+    uint8_t rssi;
+    uint8_t remote_rssi;
+    uint8_t noise;
+    uint8_t remote_noise;
+    uint16_t rxerrors;
+    uint16_t fixed;
+    uint8_t txbuf;
+};
+
 /********** SYSTEM MESSAGES, ID > 0x80 **********/
 
 /* --- TIME - TIME STAMP --- */
@@ -311,6 +323,8 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
 	LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
 	LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
+	LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"),
+
 	/* system-level messages, ID >= 0x80 */
 	// FMT: don't write format of format message, it's useless
 	LOG_FORMAT(TIME, "Q", "StartTime"),

src/modules/sensors/sensor_params.c

@@ -42,13 +42,10 @@
  */
 
 #include <nuttx/config.h>
-
 #include <systemlib/param/param.h>
 
 /**
- * Gyro X offset
- *
- * This is an X-axis offset for the gyro. Adjust it according to the calibration data.
+ * Gyro X-axis offset
  *
  * @min -10.0
  * @max 10.0
@@ -57,7 +54,7 @@
 PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f);
 
 /**
- * Gyro Y offset
+ * Gyro Y-axis offset
  *
  * @min -10.0
  * @max 10.0
@@ -66,7 +63,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f);
 PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f);
 
 /**
- * Gyro Z offset
+ * Gyro Z-axis offset
  *
  * @min -5.0
  * @max 5.0
@@ -75,9 +72,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f);
 PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f);
 
 /**
- * Gyro X scaling
- *
- * X-axis scaling.
+ * Gyro X-axis scaling factor
  *
  * @min -1.5
  * @max 1.5
@@ -86,9 +81,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f);
 PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f);
 
 /**
- * Gyro Y scaling
- *
- * Y-axis scaling.
+ * Gyro Y-axis scaling factor
  *
  * @min -1.5
  * @max 1.5
@@ -97,9 +90,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f);
 PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f);
 
 /**
- * Gyro Z scaling
- *
- * Z-axis scaling.
+ * Gyro Z-axis scaling factor
  *
  * @min -1.5
  * @max 1.5
@@ -107,10 +98,9 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f);
  */
 PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f);
 
+
 /**
- * Magnetometer X offset
- *
- * This is an X-axis offset for the magnetometer.
+ * Magnetometer X-axis offset
  *
  * @min -500.0
  * @max 500.0
@@ -119,9 +109,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f);
 PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f);
 
 /**
- * Magnetometer Y offset
- *
- * This is an Y-axis offset for the magnetometer.
+ * Magnetometer Y-axis offset
  *
  * @min -500.0
  * @max 500.0
@@ -130,9 +118,7 @@ PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f);
 PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f);
 
 /**
- * Magnetometer Z offset
- *
- * This is an Z-axis offset for the magnetometer.
+ * Magnetometer Z-axis offset
  *
  * @min -500.0
  * @max 500.0
@@ -140,24 +126,134 @@ PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f);
  */
 PARAM_DEFINE_FLOAT(SENS_MAG_ZOFF, 0.0f);
 
+/**
+ * Magnetometer X-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_MAG_XSCALE, 1.0f);
+
+/**
+ * Magnetometer Y-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_MAG_YSCALE, 1.0f);
+
+/**
+ * Magnetometer Z-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_MAG_ZSCALE, 1.0f);
 
+
+/**
+ * Accelerometer X-axis offset
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_ACC_XOFF, 0.0f);
+
+/**
+ * Accelerometer Y-axis offset
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_ACC_YOFF, 0.0f);
+
+/**
+ * Accelerometer Z-axis offset
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_ACC_ZOFF, 0.0f);
 
+/**
+ * Accelerometer X-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f);
+
+/**
+ * Accelerometer Y-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f);
+
+/**
+ * Accelerometer Z-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
 
+
+/**
+ * Differential pressure sensor offset
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f);
+
+/**
+ * Differential pressure sensor analog enabled
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0);
 
+
+/**
+ * Board rotation
+ *
+ * This parameter defines the rotation of the FMU board relative to the platform.
+ * Possible values are:
+ *    0 = No rotation
+ *    1 = Yaw 45°
+ *    2 = Yaw 90°
+ *    3 = Yaw 135°
+ *    4 = Yaw 180°
+ *    5 = Yaw 225°
+ *    6 = Yaw 270°
+ *    7 = Yaw 315°
+ *    8 = Roll 180°
+ *    9 = Roll 180°, Yaw 45°
+ *   10 = Roll 180°, Yaw 90°
+ *   11 = Roll 180°, Yaw 135°
+ *   12 = Pitch 180°
+ *   13 = Roll 180°, Yaw 225°
+ *   14 = Roll 180°, Yaw 270°
+ *   15 = Roll 180°, Yaw 315°
+ *   16 = Roll 90°
+ *   17 = Roll 90°, Yaw 45°
+ *   18 = Roll 90°, Yaw 90°
+ *   19 = Roll 90°, Yaw 135°
+ *   20 = Roll 270°
+ *   21 = Roll 270°, Yaw 45°
+ *   22 = Roll 270°, Yaw 90°
+ *   23 = Roll 270°, Yaw 135°
+ *   24 = Pitch 90°
+ *   25 = Pitch 270°
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
+
+/**
+ * External magnetometer rotation
+ *
+ * This parameter defines the rotation of the external magnetometer relative
+ * to the platform (not relative to the FMU).
+ * See SENS_BOARD_ROT for possible values.
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0);
 
+
 /**
  * RC Channel 1 Minimum
  *
@@ -367,20 +463,52 @@ PARAM_DEFINE_FLOAT(RC18_DZ, 0.0f);
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */
 #endif
-PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind */
 
+/**
+ * DSM binding trigger.
+ *
+ * -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind
+ *
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_DSM_BIND, -1);
+
+
+/**
+ * Scaling factor for battery voltage sensor on PX4IO.
+ *
+ * @group Battery Calibration
+ */
 PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000);
+
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+/**
+ * Scaling factor for battery voltage sensor on FMU v2.
+ *
+ * @group Battery Calibration
+ */
 PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
 #else
-/* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
-/* PX4IOAR: 0.00838095238 */
-/* FMU standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 */
-/* FMU with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) */
+/**
+ * Scaling factor for battery voltage sensor on FMU v1.
+ *
+ * FMUv1 standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659
+ * FMUv1 with PX4IO: 0.00459340659
+ * FMUv1 with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) = 0.00838095238
+ *
+ * @group Battery Calibration
+ */
 PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f);
 #endif
+
+/**
+ * Scaling factor for battery current sensor.
+ *
+ * @group Battery Calibration
+ */
 PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124);	/* scaling for 3DR power brick */
 
+
 /**
  * Roll control channel mapping.
  *
@@ -446,22 +574,127 @@ PARAM_DEFINE_INT32(RC_MAP_YAW, 4);
  * @group Radio Calibration
  */
 PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0);
+
+/**
+ * Return switch channel mapping.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0);
+
+/**
+ * Assist switch channel mapping.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0);
+
+/**
+ * Mission switch channel mapping.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0);
 
 //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
 
+/**
+ * Flaps channel mapping.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0);
 
-PARAM_DEFINE_INT32(RC_MAP_AUX1, 0);	/**< default function: camera pitch */
-PARAM_DEFINE_INT32(RC_MAP_AUX2, 0);	/**< default function: camera roll */
-PARAM_DEFINE_INT32(RC_MAP_AUX3, 0);	/**< default function: camera azimuth / yaw */
+/**
+ * Auxiliary switch 1 channel mapping.
+ *
+ * Default function: Camera pitch
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_MAP_AUX1, 0);
 
+/**
+ * Auxiliary switch 2 channel mapping.
+ *
+ * Default function: Camera roll
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_MAP_AUX2, 0);	/**< default function: camera roll */
+
+/**
+ * Auxiliary switch 3 channel mapping.
+ *
+ * Default function: Camera azimuth / yaw
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_MAP_AUX3, 0);
+
+
+/**
+ * Roll scaling factor
+ *
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f);
+
+/**
+ * Pitch scaling factor
+ *
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f);
+
+/**
+ * Yaw scaling factor
+ *
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f);
 
-PARAM_DEFINE_INT32(RC_FS_CH, 0);	/**< RC failsafe channel, 0 = disable */
-PARAM_DEFINE_INT32(RC_FS_MODE, 0);	/**< RC failsafe mode: 0 = too low means signal loss, 1 = too high means signal loss */
-PARAM_DEFINE_FLOAT(RC_FS_THR, 800);	/**< RC failsafe PWM threshold */
+
+/**
+ * Failsafe channel mapping.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_FS_CH, 0);
+
+/**
+ * Failsafe channel mode.
+ *
+ * 0 = too low means signal loss,
+ * 1 = too high means signal loss
+ *
+ * @min 0
+ * @max 1
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_FS_MODE, 0);
+
+/**
+ * Failsafe channel PWM threshold.
+ *
+ * @min 0
+ * @max 1
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_FLOAT(RC_FS_THR, 800);

src/modules/systemlib/perf_counter.h

@@ -39,6 +39,8 @@
 #ifndef _SYSTEMLIB_PERF_COUNTER_H
 #define _SYSTEMLIB_PERF_COUNTER_H value
 
+#include <stdint.h>
+
 /**
  * Counter types.
  */

src/modules/systemlib/system_params.c

@@ -40,8 +40,23 @@
 #include <nuttx/config.h>
 #include <systemlib/param/param.h>
 
-// Auto-start script with index #n
+/**
+ * Auto-start script index.
+ *
+ * Defines the auto-start script used to bootstrap the system.
+ *
+ * @group System
+ */
 PARAM_DEFINE_INT32(SYS_AUTOSTART, 0);
 
-// Automatically configure default values
+/**
+ * Automatically configure default values.
+ *
+ * Set to 1 to set platform-specific parameters to their default
+ * values on next system startup. 
+ *
+ * @min 0
+ * @max 1
+ * @group System
+ */
 PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0);

src/modules/uORB/topics/telemetry_status.h

@@ -58,10 +58,10 @@ enum TELEMETRY_STATUS_RADIO_TYPE {
 struct telemetry_status_s {
 	uint64_t timestamp;
     enum TELEMETRY_STATUS_RADIO_TYPE type;  /**< type of the radio hardware     */
-	unsigned rssi;              /**< local signal strength                      */
-    unsigned remote_rssi;       /**< remote signal strength                     */
-    unsigned rxerrors;          /**< receive errors                             */
-    unsigned fixed;             /**< count of error corrected packets           */
+    uint8_t rssi;              /**< local signal strength                      */
+    uint8_t remote_rssi;       /**< remote signal strength                     */
+    uint16_t rxerrors;          /**< receive errors                             */
+    uint16_t fixed;             /**< count of error corrected packets           */
     uint8_t noise;              /**< background noise level                     */
     uint8_t remote_noise;       /**< remote background noise level              */
     uint8_t txbuf;              /**< how full the tx buffer is as a percentage  */
@@ -73,4 +73,4 @@ struct telemetry_status_s {
 
 ORB_DECLARE(telemetry_status);
 
-#endif /* TOPIC_TELEMETRY_STATUS_H */
+#endif /* TOPIC_TELEMETRY_STATUS_H */

src/systemcmds/tests/module.mk

@@ -26,6 +26,7 @@ SRCS			 = test_adc.c \
 			   test_mixer.cpp \
 			   test_mathlib.cpp \
 			   test_file.c \
+			   test_file2.c \
 			   tests_main.c \
 			   test_param.c \
 			   test_ppm_loopback.c \

src/systemcmds/tests/test_file2.c

@@ -0,0 +1,196 @@
+/****************************************************************************
+ *
+ *   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 test_file2.c
+ *
+ * File write test.
+ */
+
+#include <sys/stat.h>
+#include <dirent.h>
+#include <stdio.h>
+#include <stddef.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <systemlib/err.h>
+#include <systemlib/perf_counter.h>
+#include <string.h>
+#include <stdlib.h>
+#include <getopt.h>
+
+#define FLAG_FSYNC 1
+#define FLAG_LSEEK 2
+
+/*
+  return a predictable value for any file offset to allow detection of corruption
+ */
+static uint8_t get_value(uint32_t ofs)
+{
+	union {
+		uint32_t ofs;
+		uint8_t buf[4];
+	} u;
+	u.ofs = ofs;
+	return u.buf[ofs % 4];
+}
+
+static void test_corruption(const char *filename, uint32_t write_chunk, uint32_t write_size, uint16_t flags)
+{
+	printf("Testing on %s with write_chunk=%u write_size=%u\n", 
+	       filename, (unsigned)write_chunk, (unsigned)write_size);
+	
+	uint32_t ofs = 0;
+	int fd = open(filename, O_CREAT | O_RDWR | O_TRUNC);
+	if (fd == -1) {
+		perror(filename);
+		exit(1);
+	}
+	
+	// create a file of size write_size, in write_chunk blocks
+	uint8_t counter = 0;
+	while (ofs < write_size) {
+        uint8_t buffer[write_chunk];
+        for (uint16_t j=0; j<write_chunk; j++) {
+		buffer[j] = get_value(ofs);
+		ofs++;
+        }
+        if (write(fd, buffer, sizeof(buffer)) != sizeof(buffer)) {
+		printf("write failed at offset %u\n", ofs);
+		exit(1);
+        }
+	if (flags & FLAG_FSYNC) {
+		fsync(fd);
+	}
+        if (counter % 100 == 0) {
+		printf("write ofs=%u\r", ofs);
+        }
+        counter++;
+	}
+	close(fd);
+	
+	printf("write ofs=%u\n", ofs);
+	
+	// read and check
+	fd = open(filename, O_RDONLY);
+	if (fd == -1) {
+		perror(filename);
+		exit(1);
+	}
+
+	counter = 0;
+	ofs = 0;
+	while (ofs < write_size) {
+		uint8_t buffer[write_chunk];
+		if (counter % 100 == 0) {
+			printf("read ofs=%u\r", ofs);
+		}
+		counter++;
+		if (read(fd, buffer, sizeof(buffer)) != sizeof(buffer)) {
+			printf("read failed at offset %u\n", ofs);
+			exit(1);
+		}
+		for (uint16_t j=0; j<write_chunk; j++) {
+			if (buffer[j] != get_value(ofs)) {
+				printf("corruption at ofs=%u got %u\n", ofs, buffer[j]);
+				exit(1);
+			}
+			ofs++;
+		}
+		if (flags & FLAG_LSEEK) {
+			lseek(fd, 0, SEEK_CUR);
+		}
+	}
+	printf("read ofs=%u\n", ofs);
+	close(fd);
+	unlink(filename);
+	printf("All OK\n");    
+}
+
+static void usage(void)
+{
+	printf("test file2 [options] [filename]\n");
+	printf("\toptions:\n");
+	printf("\t-s SIZE     set file size\n");
+	printf("\t-c CHUNK    set IO chunk size\n");
+	printf("\t-F          fsync on every write\n");
+	printf("\t-L          lseek on every read\n");
+}
+
+int test_file2(int argc, char *argv[])
+{
+	int opt;
+	uint16_t flags = 0;
+	const char *filename = "/fs/microsd/testfile2.dat";
+	uint32_t write_chunk = 64;
+	uint32_t write_size = 5*1024;
+
+	while ((opt = getopt(argc, argv, "c:s:FLh")) != EOF) {
+		switch (opt) {
+		case 'F':
+			flags |= FLAG_FSYNC;
+			break;
+		case 'L':
+			flags |= FLAG_LSEEK;
+			break;
+		case 's':
+			write_size = strtoul(optarg, NULL, 0);
+			break;
+		case 'c':
+			write_chunk = strtoul(optarg, NULL, 0);
+			break;
+		case 'h':
+		default:
+			usage();
+			exit(1);
+		}
+	}
+
+	argc -= optind;
+	argv += optind;
+
+	if (argc > 0) {
+		filename = argv[0];
+	}
+
+	/* check if microSD card is mounted */
+	struct stat buffer;
+	if (stat("/fs/microsd/", &buffer)) {
+		warnx("no microSD card mounted, aborting file test");
+		return 1;
+	}
+
+        test_corruption(filename, write_chunk, write_size, flags);
+        return 0;
+}
+

src/systemcmds/tests/test_mount.c

@@ -141,8 +141,8 @@ test_mount(int argc, char *argv[])
 			/* announce mode switch */
 			if (it_left_fsync_prev != it_left_fsync && it_left_fsync == 0) {
 				warnx("\n SUCCESSFULLY PASSED FSYNC'ED WRITES, CONTINUTING WITHOUT FSYNC");
-				fsync(stdout);
-				fsync(stderr);
+				fsync(fileno(stdout));
+				fsync(fileno(stderr));
 				usleep(20000);
 			}
 
@@ -162,7 +162,7 @@ test_mount(int argc, char *argv[])
 	}
 
 	char buf[64];
-	int wret = sprintf(buf, "TEST: %d %d ", it_left_fsync, it_left_abort);
+	(void)sprintf(buf, "TEST: %d %d ", it_left_fsync, it_left_abort);
 	lseek(cmd_fd, 0, SEEK_SET);
 	write(cmd_fd, buf, strlen(buf) + 1);
 	fsync(cmd_fd);
@@ -174,8 +174,8 @@ test_mount(int argc, char *argv[])
 
 		printf("\n\n====== FILE TEST: %u bytes chunks (%s) ======\n", chunk_sizes[c], (it_left_fsync > 0) ? "FSYNC" : "NO FSYNC");
 		printf("unpower the system immediately (within 0.5s) when the hash (#) sign appears\n");
-		fsync(stdout);
-		fsync(stderr);
+		fsync(fileno(stdout));
+		fsync(fileno(stderr));
 		usleep(50000);
 
 		for (unsigned a = 0; a < alignments; a++) {
@@ -185,22 +185,20 @@ test_mount(int argc, char *argv[])
 			uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64)));
 
 			/* fill write buffer with known values */
-			for (int i = 0; i < sizeof(write_buf); i++) {
+			for (unsigned i = 0; i < sizeof(write_buf); i++) {
 				/* this will wrap, but we just need a known value with spacing */
 				write_buf[i] = i+11;
 			}
 
 			uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64)));
-			hrt_abstime start, end;
 
 			int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
 
-			start = hrt_absolute_time();
 			for (unsigned i = 0; i < iterations; i++) {
 
 				int wret = write(fd, write_buf + a, chunk_sizes[c]);
 
-				if (wret != chunk_sizes[c]) {
+				if (wret != (int)chunk_sizes[c]) {
 					warn("WRITE ERROR!");
 
 					if ((0x3 & (uintptr_t)(write_buf + a)))
@@ -214,8 +212,8 @@ test_mount(int argc, char *argv[])
 					fsync(fd);
 				} else {
 					printf("#");
-					fsync(stdout);
-					fsync(stderr);
+					fsync(fileno(stdout));
+					fsync(fileno(stderr));
 				}
 			}
 
@@ -224,12 +222,10 @@ test_mount(int argc, char *argv[])
 			}
 
 			printf(".");
-			fsync(stdout);
-			fsync(stderr);
+			fsync(fileno(stdout));
+			fsync(fileno(stderr));
 			usleep(200000);
 
-			end = hrt_absolute_time();
-
 			close(fd);
 			fd = open("/fs/microsd/testfile", O_RDONLY);
 
@@ -237,7 +233,7 @@ test_mount(int argc, char *argv[])
 			for (unsigned i = 0; i < iterations; i++) {
 				int rret = read(fd, read_buf, chunk_sizes[c]);
 
-				if (rret != chunk_sizes[c]) {
+				if (rret != (int)chunk_sizes[c]) {
 					warnx("READ ERROR!");
 					return 1;
 				}
@@ -245,7 +241,7 @@ test_mount(int argc, char *argv[])
 				/* compare value */
 				bool compare_ok = true;
 
-				for (int j = 0; j < chunk_sizes[c]; j++) {
+				for (unsigned j = 0; j < chunk_sizes[c]; j++) {
 					if (read_buf[j] != write_buf[j + a]) {
 						warnx("COMPARISON ERROR: byte %d, align shift: %d", j, a);
 						compare_ok = false;
@@ -271,16 +267,16 @@ test_mount(int argc, char *argv[])
 		}
 	}
 
-	fsync(stdout);
-	fsync(stderr);
+	fsync(fileno(stdout));
+	fsync(fileno(stderr));
 	usleep(20000);
 
 
 
 	/* we always reboot for the next test if we get here */
 	warnx("Iteration done, rebooting..");
-	fsync(stdout);
-	fsync(stderr);
+	fsync(fileno(stdout));
+	fsync(fileno(stderr));
 	usleep(50000);
 	systemreset(false);
 

src/systemcmds/tests/tests.h

@@ -107,6 +107,7 @@ 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_file2(int argc, char *argv[]);
 extern int	test_mixer(int argc, char *argv[]);
 extern int	test_rc(int argc, char *argv[]);
 extern int	test_conv(int argc, char *argv[]);

src/systemcmds/tests/tests_main.c

@@ -104,6 +104,7 @@ const struct {
 	{"param",		test_param,	0},
 	{"bson",		test_bson,	0},
 	{"file",		test_file,	0},
+	{"file2",		test_file2,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"mixer",		test_mixer,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"rc",			test_rc,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"conv",		test_conv,	OPT_NOJIGTEST | OPT_NOALLTEST},

src/systemcmds/top/top.c

@@ -233,8 +233,8 @@ top_main(void)
 					   system_load.tasks[i].tcb->pid,
 					   CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name,
 					   (system_load.tasks[i].total_runtime / 1000),
-					   (int)(curr_loads[i] * 100),
-					   (int)(curr_loads[i] * 100000.0f - (int)(curr_loads[i] * 1000.0f) * 100),
+					   (int)(curr_loads[i] * 100.0f),
+					   (int)((curr_loads[i] * 100.0f - (int)(curr_loads[i] * 100.0f)) * 1000),
 					   stack_size - stack_free,
 					   stack_size,
 					   system_load.tasks[i].tcb->sched_priority,