Made a separate precision landing library. Now, the Precision Landing mode can use it the same as the RTL and the mission mode.

.ci/Jenkinsfile-hardware

@@ -920,6 +920,7 @@ void printTopics() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener tune_control" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_acceleration" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_air_data" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_angular_acceleration" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_angular_velocity" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_attitude" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_attitude_setpoint" || true'

.github/PULL_REQUEST_TEMPLATE.md

@@ -1,29 +1,17 @@
-<!--
+Please use [PX4 Discuss](http://discuss.px4.io/) or [Discord](https://discord.gg/dronecode) to align on pull requests if necessary. You can then open draft pull requests to get early feedback.
 
-Thank you for your contribution!
+## Describe problem solved by this pull request
+A clear and concise description of the problem this proposed change will solve. Or, what it will improve.
+E.g. For this use case I ran into...
 
-Get early feedback through
-- Dronecode Discord: https://discord.gg/dronecode
-- PX4 Discuss: http://discuss.px4.io/
-- opening a draft pr and sharing the link
+## Describe your solution
+A clear and concise description of what you have implemented.
 
--->
+## Describe possible alternatives
+A clear and concise description of alternative solutions or features you've considered.
 
-### Solved Problem
-When ... I found that ...
+## Test data / coverage
+How was it tested? What cases were covered? Logs uploaded to https://review.px4.io/ and screenshots of the important plot parts.
 
-Fixes #{Github issue ID}
-
-### Solution
-- Add ... for ...
-- Refactor ...
-
-### Alternatives
-We could also ...
-
-### Test coverage
-- Unit/integration test: ...
-- Simulation/hardware testing logs: https://review.px4.io/
-
-### Context
-Related links, screenshot before/after, video
+## Additional context
+Add any other related context or media.

Kconfig

@@ -190,7 +190,3 @@ endmenu
 menu "examples"
 source "src/examples/Kconfig"
 endmenu
-
-menu "platforms"
-source "platforms/common/Kconfig"
-endmenu

ROMFS/px4fmu_common/init.d-posix/airframes/1013_iris_vision

@@ -10,7 +10,6 @@
 # EKF2: Vision position and heading
 param set-default EKF2_AID_MASK 24
 param set-default EKF2_EV_DELAY 5
-param set-default EKF2_EV_CTRL 15
 param set-default EKF2_GPS_CTRL 0
 
 # LPE: Vision + baro

ROMFS/px4fmu_common/init.d/rcS

@@ -264,6 +264,23 @@ else
 		. $FCONFIG
 	fi
 
+	#
+	# Check if UAVCAN is enabled, default to it for ESCs.
+	#
+	if param greater -s UAVCAN_ENABLE 0
+	then
+		# Start core UAVCAN module.
+		if ! uavcan start
+		then
+			tune_control play error
+		fi
+	else
+		if param greater -s CYPHAL_ENABLE 0
+		then
+			cyphal start
+		fi
+	fi
+
 	#
 	# Start IO for PWM output or RC input if enabled
 	#
@@ -510,23 +527,6 @@ else
 	fi
 	unset BOARD_BOOTLOADER_UPGRADE
 
-	#
-	# Check if UAVCAN is enabled, default to it for ESCs.
-	#
-	if param greater -s UAVCAN_ENABLE 0
-	then
-		# Start core UAVCAN module.
-		if ! uavcan start
-		then
-			tune_control play error
-		fi
-	else
-		if param greater -s CYPHAL_ENABLE 0
-		then
-			cyphal start
-		fi
-	fi
-
 #
 # End of autostart.
 #

Tools/px4airframes/markdownout.py

@@ -51,9 +51,9 @@ div.frame_variant td, div.frame_variant th {
   text-align : left;
 }
 </style>\n\n"""
-
+ 
         type_set = set()
-
+        
         if len(image_path) > 0 and image_path[-1] != '/':
             image_path = image_path + '/'
 
@@ -62,7 +62,7 @@ div.frame_variant td, div.frame_variant th {
                result += '## %s\n\n' % group.GetClass()
                type_set.add(group.GetClass())
 
-            result += '### %s\n\n' % group.GetType()
+            result += '### %s\n\n' % group.GetName()
 
             # Display an image of the frame
             image_name = group.GetImageName()
@@ -73,11 +73,11 @@ div.frame_variant td, div.frame_variant th {
             # check if all outputs are equal for the group: if so, show them
             # only once
             all_outputs = {}
-            num_configs = len(group.GetAirframes())
-            for airframe in group.GetAirframes():
-                if not self.IsExcluded(airframe, board):
-                    for output_name in airframe.GetOutputCodes():
-                        value = airframe.GetOutputValue(output_name)
+            num_configs = len(group.GetParams())
+            for param in group.GetParams():
+                if not self.IsExcluded(param, board):
+                    for output_name in param.GetOutputCodes():
+                        value = param.GetOutputValue(output_name)
                         key_value_pair = (output_name, value)
                         if key_value_pair not in all_outputs:
                             all_outputs[key_value_pair] = 0
@@ -104,17 +104,18 @@ div.frame_variant td, div.frame_variant th {
             result += ' </thead>\n'
             result += '<tbody>\n'
 
-            for airframe in group.GetAirframes():
-                if not self.IsExcluded(airframe, board):
-                    name = airframe.GetName()
-                    airframe_id = airframe.GetId()
+            for param in group.GetParams():
+                if not self.IsExcluded(param, board):
+                    #print("generating: {0} {1}".format(param.GetName(), excluded))
+                    name = param.GetName()
+                    airframe_id = param.GetId()
                     airframe_id_entry = '<p><code>SYS_AUTOSTART</code> = %s</p>' % (airframe_id)
-                    maintainer = airframe.GetMaintainer()
+                    maintainer = param.GetMaintainer()
                     maintainer_entry = ''
                     if maintainer != '':
                         maintainer_entry = 'Maintainer: %s' % (html.escape(maintainer))
-                    url = airframe.GetFieldValue('url')
-                    name_anchor='%s_%s_%s' % (group.GetClass(),group.GetType(),name)
+                    url = param.GetFieldValue('url')
+                    name_anchor='%s_%s_%s' % (group.GetClass(),group.GetName(),name)
                     name_anchor=name_anchor.replace(' ','_').lower()
                     name_anchor=name_anchor.replace('"','_').lower()
                     name_anchor='id="%s"' % name_anchor
@@ -123,8 +124,8 @@ div.frame_variant td, div.frame_variant th {
                         name_entry = '<a href="%s">%s</a>' % (url, name)
                     outputs = '<ul>'
                     has_outputs = False
-                    for output_name in airframe.GetOutputCodes():
-                        value = airframe.GetOutputValue(output_name)
+                    for output_name in param.GetOutputCodes():
+                        value = param.GetOutputValue(output_name)
                         valstrs = value.split(";")
                         key_value_pair = (output_name, value)
                         if all_outputs[key_value_pair] < num_configs:
@@ -151,9 +152,9 @@ div.frame_variant td, div.frame_variant th {
 
         self.output = result
 
-    def IsExcluded(self, airframe, board):
-        for code in airframe.GetArchCodes():
-            if "CONFIG_ARCH_BOARD_{0}".format(code) == board and airframe.GetArchValue(code) == "exclude":
+    def IsExcluded(self, param, board):
+        for code in param.GetArchCodes():
+            if "CONFIG_ARCH_BOARD_{0}".format(code) == board and param.GetArchValue(code) == "exclude":
                 return True
         return False
 

Tools/px4airframes/rcout.py

@@ -3,9 +3,6 @@ import codecs
 import os
 
 class RCOutput():
-    """
-        Generates RC scripts for the airframes
-    """
     def __init__(self, groups, board, post_start=False):
 
         result = (  "#\n"
@@ -37,33 +34,33 @@ class RCOutput():
         result += "set AIRFRAME none\n"
         result += "\n"
         for group in groups:
-            result += "# GROUP: %s\n\n" % group.GetType()
-            for airframe in group.GetAirframes():
+            result += "# GROUP: %s\n\n" % group.GetName()
+            for param in group.GetParams():
                 excluded = False
-                for code in airframe.GetArchCodes():
-                    if "{0}".format(code) == board and airframe.GetArchValue(code) == "exclude":
+                for code in param.GetArchCodes():
+                    if "{0}".format(code) == board and param.GetArchValue(code) == "exclude":
                         excluded = True
                 if excluded:
                     continue
 
                 if post_start:
                     # Path to post-start sript
-                    path = airframe.GetPostPath()
+                    path = param.GetPostPath()
                 else:
                     # Path to start script
-                    path = airframe.GetPath()
+                    path = param.GetPath()
 
                 if not path:
                     continue
 
                 path = os.path.split(path)[1]
 
-                id_val = airframe.GetId()
-                name = airframe.GetFieldValue("short_desc")
-                long_desc = airframe.GetFieldValue("long_desc")
+                id_val = param.GetId()
+                name = param.GetFieldValue("short_desc")
+                long_desc = param.GetFieldValue("long_desc")
 
                 result +=   "#\n"
-                result +=   "# %s\n" % airframe.GetName()
+                result +=   "# %s\n" % param.GetName()
                 result +=   "if param compare SYS_AUTOSTART %s\n" % id_val
                 result +=   "then\n"
                 result +=   "\tset AIRFRAME %s\n" % path

Tools/px4airframes/srcparser.py

@@ -2,38 +2,31 @@ import sys
 import re
 import os
 
-class AirframeGroup(object):
+class ParameterGroup(object):
     """
-    Airframe group
-
-    type: specific vehicle type (e.g. VTOL Tiltrotor, VTOL Quadrotor, etc.)
-    class: vehicle class (e.g. Multicopter, Fixed Wing, etc.)
+    Single parameter group
     """
-    def __init__(self, type, af_class):
-        self.type = type
+    def __init__(self, name, af_class):
+        self.name = name
         self.af_class = af_class
-        self.airframes = []
+        self.params = []
 
 
-    def AddAirframe(self, airframe):
+    def AddParameter(self, param):
         """
-        Add airframe to the airframe group
+        Add parameter to the group
         """
-        self.airframes.append(airframe)
+        self.params.append(param)
 
-    def GetType(self):
+    def GetName(self):
         """
-        Get airframe group's vehicle type
-
-        e.g. VTOL Tiltrotor, VTOL Quadrotor, etc.
+        Get parameter group name
         """
-        return self.type
+        return self.name
 
     def GetClass(self):
         """
-        Get airframe group's vehicle class
-
-        e.g. Multicopter, Fixed Wing, etc.
+        Get parameter group vehicle type.
         """
         return self.af_class
 
@@ -41,84 +34,86 @@ class AirframeGroup(object):
         """
         Get parameter group image base name (w/o extension)
         """
-        if (self.type == "Standard Plane"):
+        if (self.name == "Standard Plane"):
             return "Plane"
-        elif (self.type == "Flying Wing"):
+        elif (self.name == "Flying Wing"):
             return "FlyingWing"
-        elif (self.type == "Quadrotor x"):
+        elif (self.name == "Quadrotor x"):
             return "QuadRotorX"
-        elif (self.type == "Quadrotor +"):
+        elif (self.name == "Quadrotor +"):
             return "QuadRotorPlus"
-        elif (self.type == "Hexarotor x"):
+        elif (self.name == "Hexarotor x"):
             return "HexaRotorX"
-        elif (self.type == "Hexarotor +"):
+        elif (self.name == "Hexarotor +"):
             return "HexaRotorPlus"
-        elif (self.type == "Octorotor +"):
+        elif (self.name == "Octorotor +"):
             return "OctoRotorPlus"
-        elif (self.type == "Octorotor x"):
+        elif (self.name == "Octorotor x"):
             return "OctoRotorX"
-        elif (self.type == "Octorotor Coaxial"):
+        elif (self.name == "Octorotor Coaxial"):
             return "OctoRotorXCoaxial"
-        elif (self.type == "Octo Coax Wide"):
+        elif (self.name == "Octo Coax Wide"):
             return "OctoRotorXCoaxial"
-        elif (self.type == "Quadrotor Wide"):
+        elif (self.name == "Quadrotor Wide"):
             return "QuadRotorWide"
-        elif (self.type == "Quadrotor H"):
+        elif (self.name == "Quadrotor H"):
             return "QuadRotorH"
-        elif (self.type == "Dodecarotor cox"):
+        elif (self.name == "Dodecarotor cox"):
             return "DodecaRotorXCoaxial"
-        elif (self.type == "Simulation"):
+        elif (self.name == "Simulation"):
             return "AirframeSimulation"
-        elif (self.type == "Plane A-Tail"):
+        elif (self.name == "Plane A-Tail"):
             return "PlaneATail"
-        elif (self.type == "Plane V-Tail"):
+        elif (self.name == "Plane V-Tail"):
             return "PlaneVTail"
-        elif (self.type == "VTOL Duo Tailsitter"):
+        elif (self.name == "VTOL Duo Tailsitter"):
             return "VTOLDuoRotorTailSitter"
-        elif (self.type == "Standard VTOL"):
+        elif (self.name == "Standard VTOL"):
             return "VTOLPlane"
-        elif (self.type == "VTOL Quad Tailsitter"):
+        elif (self.name == "VTOL Quad Tailsitter"):
             return "VTOLQuadRotorTailSitter"
-        elif (self.type == "VTOL Tiltrotor"):
+        elif (self.name == "VTOL Tiltrotor"):
             return "VTOLTiltRotor"
-        elif (self.type == "VTOL Octoplane"):
+        elif (self.name == "VTOL Octoplane"):
             return "VTOLPlaneOcto"
-        elif (self.type == "Coaxial Helicopter"):
+        elif (self.name == "Coaxial Helicopter"):
             return "HelicopterCoaxial"
-        elif (self.type == "Helicopter"):
+        elif (self.name == "Helicopter"):
             return "Helicopter"
-        elif (self.type == "Hexarotor Coaxial"):
+        elif (self.name == "Hexarotor Coaxial"):
             return "Y6B"
-        elif (self.type == "Y6A"):
+        elif (self.name == "Y6A"):
             return "Y6A"
-        elif (self.type == "Tricopter Y-"):
+        elif (self.name == "Tricopter Y-"):
             return "YMinus"
-        elif (self.type == "Tricopter Y+"):
+        elif (self.name == "Tricopter Y+"):
             return "YPlus"
-        elif (self.type == "Autogyro"):
+        elif (self.name == "Autogyro"):
             return "Autogyro"
-        elif (self.type == "Airship"):
+        elif (self.name == "Airship"):
             return "Airship"
-        elif (self.type == "Rover"):
+        elif (self.name == "Rover"):
             return "Rover"
-        elif (self.type == "Boat"):
+        elif (self.name == "Boat"):
             return "Boat"
-        elif (self.type == "Balloon"):
+        elif (self.name == "Balloon"):
             return "Balloon"
-        elif (self.type == "Vectored 6 DOF UUV"):
+        elif (self.name == "Vectored 6 DOF UUV"):
             return "Vectored6DofUUV"
         return "AirframeUnknown"
 
-    def GetAirframes(self):
+    def GetParams(self):
         """
-        Returns the parsed list of airframes objects. Note that returned
-        object is not a copy. Modifications affect state of the parser.
+        Returns the parsed list of parameters. Every parameter is a Parameter
+        object. Note that returned object is not a copy. Modifications affect
+        state of the parser.
         """
-        return sorted(self.airframes, key=lambda x: x.GetId())
 
-class Airframe(object):
+        return sorted(self.params, key=lambda x: x.GetId())
+
+class Parameter(object):
     """
-    Single Airframe definition
+    Single parameter
     """
 
     # Define sorting order of the fields
@@ -293,7 +288,7 @@ class SourceParser(object):
     }
 
     def __init__(self):
-        self.airframe_groups = {}
+        self.param_groups = {}
 
     def GetSupportedExtensions(self):
         """
@@ -352,10 +347,10 @@ class SourceParser(object):
                             tag, desc = m.group(1, 2)
                             if (tag == "output"):
                                 key, text = desc.split(' ', 1)
-                                outputs[key] = text
+                                outputs[key] = text;
                             elif (tag == "board"):
                                 key, text = desc.split(' ', 1)
-                                archs[key] = text
+                                archs[key] = text;
                             else:
                                 tags[tag] = desc
                             current_tag = tag
@@ -432,7 +427,7 @@ class SourceParser(object):
             post_path = None
 
         # We already know this is an airframe config, so add it
-        airframe = Airframe(path, post_path, airframe_name, airframe_type, airframe_class, airframe_id, maintainer)
+        param = Parameter(path, post_path, airframe_name, airframe_type, airframe_class, airframe_id, maintainer)
 
         # Done with file, store
         for tag in tags:
@@ -445,24 +440,24 @@ class SourceParser(object):
             if tag == "name":
                 airframe_name = tags[tag]
             else:
-                airframe.SetField(tag, tags[tag])
+                param.SetField(tag, tags[tag])
 
         # Store outputs
         for output in outputs:
-            airframe.SetOutput(output, outputs[output])
+            param.SetOutput(output, outputs[output])
 
         # Store outputs
         for arch in archs:
-            airframe.SetArch(arch, archs[arch])
+            param.SetArch(arch, archs[arch])
 
         # Store the parameter
 
         # Create a class-specific airframe group. This is needed to catch cases where an airframe type might cross classes (e.g. simulation)
-        class_group_identifier=airframe_type + airframe_class
-        if class_group_identifier not in self.airframe_groups:
-            #self.airframe_groups[airframe_type] = ParameterGroup(airframe_type)  #HW TEST REMOVE
-            self.airframe_groups[class_group_identifier] = AirframeGroup(airframe_type, airframe_class)
-        self.airframe_groups[class_group_identifier].AddAirframe(airframe)
+        class_group_identifier=airframe_type+airframe_class
+        if class_group_identifier not in self.param_groups:
+            #self.param_groups[airframe_type] = ParameterGroup(airframe_type)  #HW TEST REMOVE
+            self.param_groups[class_group_identifier] = ParameterGroup(airframe_type, airframe_class)
+        self.param_groups[class_group_identifier].AddParameter(param)
 
         return True
 
@@ -478,8 +473,8 @@ class SourceParser(object):
         Validates the airframe meta data.
         """
         seenParamNames = []
-        for group in self.GetAirframeGroups():
-            for param in group.GetAirframes():
+        for group in self.GetParamGroups():
+            for param in group.GetParams():
                 name  = param.GetName()
                 board = param.GetFieldValue("board")
                 # Check for duplicates
@@ -492,27 +487,27 @@ class SourceParser(object):
 
         return True
 
-    def GetAirframeGroups(self):
+    def GetParamGroups(self):
         """
-        Returns the parsed list of Airframe groups. Every Airframe is an Airframe
+        Returns the parsed list of parameters. Every parameter is a Parameter
         object. Note that returned object is not a copy. Modifications affect
         state of the parser.
         """
-        groups = self.airframe_groups.values()
-        groups = sorted(groups, key=lambda x: x.GetType())
+        groups = self.param_groups.values()
+        groups = sorted(groups, key=lambda x: x.GetName())
         groups = sorted(groups, key=lambda x: x.GetClass())
-        groups = sorted(groups, key=lambda x: self.priority.get(x.GetType(), 0), reverse=True)
+        groups = sorted(groups, key=lambda x: self.priority.get(x.GetName(), 0), reverse=True)
 
         #Rename duplicate groups to include the class (creating unique headings in page TOC)
         duplicate_test=set()
         duplicate_set=set()
         for group in groups:
-            if group.GetType() in duplicate_test:
-                duplicate_set.add(group.GetType())
+            if group.GetName() in duplicate_test:
+                duplicate_set.add(group.GetName())
             else:
-                duplicate_test.add(group.GetType() )
+                duplicate_test.add(group.GetName() )
         for group in groups:
-            if group.GetType() in duplicate_set:
-                group.name=group.GetType()+' (%s)' % group.GetClass()
+            if group.GetName() in duplicate_set:
+                group.name=group.GetName()+' (%s)' % group.GetClass()
 
         return groups

Tools/px4airframes/xmlout.py

@@ -28,28 +28,28 @@ class XMLOutput():
         xml_version.text = "1"
         for group in groups:
             xml_group = ET.SubElement(xml_parameters, "airframe_group")
-            xml_group.attrib["name"] = group.GetType()
+            xml_group.attrib["name"] = group.GetName()
             xml_group.attrib["image"] = group.GetImageName()
-            for airframe in group.GetAirframes():
+            for param in group.GetParams():
 
                 # check if there is an exclude tag for this airframe
                 excluded = False
-                for code in airframe.GetArchCodes():
-                    if "CONFIG_ARCH_BOARD_{0}".format(code) == board and airframe.GetArchValue(code) == "exclude":
+                for code in param.GetArchCodes():
+                    if "CONFIG_ARCH_BOARD_{0}".format(code) == board and param.GetArchValue(code) == "exclude":
                         excluded = True
 
                 if not excluded:
-                    #print("generating: {0} {1}".format(airframe.GetName(), excluded))
+                    #print("generating: {0} {1}".format(param.GetName(), excluded))
                     xml_param = ET.SubElement(xml_group, "airframe")
-                    xml_param.attrib["name"] = airframe.GetName()
-                    xml_param.attrib["id"] = airframe.GetId()
-                    xml_param.attrib["maintainer"] = airframe.GetMaintainer()
-                    for code in airframe.GetFieldCodes():
-                        value = airframe.GetFieldValue(code)
+                    xml_param.attrib["name"] = param.GetName()
+                    xml_param.attrib["id"] = param.GetId()
+                    xml_param.attrib["maintainer"] = param.GetMaintainer()
+                    for code in param.GetFieldCodes():
+                        value = param.GetFieldValue(code)
                         xml_field = ET.SubElement(xml_param, code)
                         xml_field.text = value
-                    for code in airframe.GetOutputCodes():
-                        value = airframe.GetOutputValue(code)
+                    for code in param.GetOutputCodes():
+                        value = param.GetOutputValue(code)
                         valstrs = value.split(";")
                         xml_field = ET.SubElement(xml_param, "output")
                         xml_field.attrib["name"] = code

Tools/px_process_airframes.py

@@ -35,11 +35,12 @@
 #
 # PX4 airframe config processor (main executable file)
 #
-# This tool scans the PX4 ROMFS directory for declarations of airframes
+# This tool scans the PX4 ROMFS code for declarations of airframes
+#
+# Currently supported formats are:
+#   * XML for the parametric UI generator
+#   * Markdown for the PX4 dev guide (https://github.com/PX4/Devguide)
 #
-# Currently supported output formats are:
-#   * XML for the parametric UI generator (Used in QGC)
-#   * Markdown for the PX4 User guide (https://github.com/PX4/PX4-user_guide)
 #
 
 from __future__ import print_function
@@ -103,31 +104,31 @@ def main():
     # We can't validate yet
     # if not parser.Validate():
     #     sys.exit(1)
-    airframe_groups = parser.GetAirframeGroups()
+    param_groups = parser.GetParamGroups()
 
     # Output to XML file
     if args.xml:
         if args.verbose: print("Creating XML file " + args.xml)
-        out = xmlout.XMLOutput(airframe_groups, args.board)
+        out = xmlout.XMLOutput(param_groups, args.board)
         out.Save(args.xml)
 
     # Output to markdown file
     if args.markdown:
         if args.verbose: print("Creating markdown file " + args.markdown)
-        out = markdownout.MarkdownTablesOutput(airframe_groups, args.board, args.image_path)
+        out = markdownout.MarkdownTablesOutput(param_groups, args.board, args.image_path)
         out.Save(args.markdown)
 
     # Output to start scripts
     if args.start_script:
         # Airframe start script
         if args.verbose: print("Creating start script " + args.start_script)
-        out = rcout.RCOutput(airframe_groups, args.board)
+        out = rcout.RCOutput(param_groups, args.board)
         out.Save(args.start_script)
 
         # Airframe post-start script
         post_start_script = args.start_script + '.post'
         if args.verbose: print("Creating post-start script " + post_start_script)
-        out_post = rcout.RCOutput(airframe_groups, args.board, post_start=True)
+        out_post = rcout.RCOutput(param_groups, args.board, post_start=True)
         out_post.Save(post_start_script)
 
     if (args.verbose): print("All done!")

boards/ark/fmu-v6x/src/mtd.cpp

@@ -71,8 +71,8 @@ static const px4_mtd_entry_t base_eeprom = {
 	.npart = 2,
 	.partd = {
 		{
-			.type = MTD_MFT_VER,
-			.path = "/fs/mtd_mft_ver",
+			.type = MTD_MFT,
+			.path = "/fs/mtd_mft",
 			.nblocks = 248
 		},
 		{
@@ -86,17 +86,12 @@ static const px4_mtd_entry_t base_eeprom = {
 
 static const px4_mtd_entry_t imu_eeprom = {
 	.device = &i2c4,
-	.npart = 3,
+	.npart = 2,
 	.partd = {
 		{
 			.type = MTD_CALDATA,
 			.path = "/fs/mtd_caldata",
-			.nblocks = 240
-		},
-		{
-			.type = MTD_MFT_REV,
-			.path = "/fs/mtd_mft_rev",
-			.nblocks = 8
+			.nblocks = 248
 		},
 		{
 			.type = MTD_ID,

boards/modalai/voxl2-slpi/src/init.c

@@ -1,35 +0,0 @@
-/****************************************************************************
- *
- * Copyright (C) 2022 ModalAI, Inc. 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.
- *
- ****************************************************************************/
-#include "board_config.h"
-
-// Place holder for VOXL2-specific early startup code

boards/modalai/voxl2/README.md

@@ -25,10 +25,10 @@ The full instructions are available here:
 - Clone the repo (Don't forget to update and initialize all submodules)
 - In the top level directory
 ```
-px4$ boards/modalai/voxl2/scripts/run-docker.sh
-root@9373fa1401b8:/usr/local/workspace# boards/modalai/voxl2/scripts/clean.sh
-root@9373fa1401b8:/usr/local/workspace# boards/modalai/voxl2/scripts/build-apps.sh
-root@9373fa1401b8:/usr/local/workspace# boards/modalai/voxl2/scripts/build-slpi.sh
+px4$ boards/modalai/voxl2/run-docker.sh
+root@9373fa1401b8:/usr/local/workspace# boards/modalai/voxl2/clean.sh
+root@9373fa1401b8:/usr/local/workspace# boards/modalai/voxl2/build-posix.sh
+root@9373fa1401b8:/usr/local/workspace# boards/modalai/voxl2/build-qurt.sh
 root@9373fa1401b8:/usr/local/workspace# exit
 ```
 
@@ -37,7 +37,7 @@ root@9373fa1401b8:/usr/local/workspace# exit
 Once the DSP and Linux images have been built they can be installed on a VOXL 2
 board using ADB. There is a script to do this.
 ```
-px4$ boards/modalai/voxl2/scripts/install-voxl.sh
+px4$ boards/modalai/voxl2/install-voxl.sh
 ```
 
 ## Running PX4 on VOXL 2
@@ -66,17 +66,10 @@ INFO  [px4] Startup script returned successfully
 pxh>
 ```
 
-## Notes
-
-You cannot cleanly shutdown PX4 with the shutdown command on VOXL 2. You have
-to power cycle the board and restart everything.
-
 ## Tips
 
 Start with a VOXL 2 that only has the system image installed, not the SDK
 
-Run the command ```voxl-px4 -s``` on target to run the self-test
-
 In order to see DSP specific debug messages the mini-dm tool in the Hexagon SDK
 can be used:
 ```

boards/modalai/voxl2/build-posix.sh

@@ -1,11 +1,12 @@
 #!/bin/bash
 
-echo "*** Starting apps processor build ***"
+echo "*** Starting posix build ***"
 
 source /home/build-env.sh
 
-make modalai_voxl2
+make modalai_voxl2_default
 
 cat build/modalai_voxl2_default/src/lib/version/build_git_version.h
 
-echo "*** End of apps processor build ***"
+
+echo "*** End of posix build ***"

boards/modalai/voxl2/build-qurt.sh

@@ -0,0 +1,11 @@
+#!/bin/bash
+
+echo "*** Starting qurt build ***"
+
+source /home/build-env.sh
+
+make modalai_voxl2_qurt
+
+cat build/modalai_voxl2_default/src/lib/version/build_git_version.h
+
+echo "*** End of qurt build ***"

boards/modalai/voxl2/cmake/link_libraries.cmake

@@ -1,7 +0,0 @@
-
-# libfc_sensor.so is provided in the Docker build environment
-target_link_libraries(px4 PRIVATE
-		/home/libfc_sensor.so
-		px4_layer
-		${module_libraries}
-)

boards/modalai/voxl2/cmake/voxl2_posix.cmake

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2022 ModalAI, Inc. All rights reserved.
+# Copyright (c) 2022 ModalAI, Inc. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -30,8 +30,25 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
+#
+# Overview:
+# Voxl2 PX4 is built in 2 parts, the part that runs on the
+# application (apps) processor, and the library that is loaded on the DSP.
+#
+############################################################################
 
-add_library(drivers_board
-	board_config.h
-	init.c
-    )
+include(px4_git)
+
+list(APPEND CMAKE_MODULE_PATH
+	"${PX4_SOURCE_DIR}/platforms/posix/cmake"
+)
+
+# set(DISABLE_PARAMS_MODULE_SCOPING TRUE)
+
+add_definitions(-DORB_COMMUNICATOR)
+
+set(CONFIG_PARAM_SERVER "1")
+
+add_definitions( -D__PX4_LINUX )
+
+include(CMakeParseArguments)

boards/modalai/voxl2/cmake/voxl2_qurt.cmake

@@ -0,0 +1,43 @@
+############################################################################
+#
+# Copyright (c) 2022 ModalAI, Inc. All rights reserved.
+#
+############################################################################
+#
+# This cmake config builds for QURT which is the operating system running on
+# the DSP side of VOXL 2
+#
+# Required environment variables:
+#	HEXAGON_TOOLS_ROOT
+#	HEXAGON_SDK_ROOT
+#
+############################################################################
+
+if ("$ENV{HEXAGON_SDK_ROOT}" STREQUAL "")
+	message(FATAL_ERROR "Enviroment variable HEXAGON_SDK_ROOT must be set")
+else()
+	set(HEXAGON_SDK_ROOT $ENV{HEXAGON_SDK_ROOT})
+endif()
+
+if ("$ENV{HEXAGON_TOOLS_ROOT}" STREQUAL "")
+	message(FATAL_ERROR "Environment variable HEXAGON_TOOLS_ROOT must be set")
+else()
+	set(HEXAGON_TOOLS_ROOT $ENV{HEXAGON_TOOLS_ROOT})
+endif()
+
+include(px4_git)
+
+list(APPEND CMAKE_MODULE_PATH
+	"${PX4_SOURCE_DIR}/platforms/qurt/cmake"
+)
+
+include(Toolchain-qurt)
+include(qurt_reqs)
+
+include_directories(${HEXAGON_SDK_INCLUDES})
+
+add_definitions(-DORB_COMMUNICATOR)
+
+set(CONFIG_PARAM_CLIENT "1")
+
+set(DISABLE_PARAMS_MODULE_SCOPING TRUE)

boards/modalai/voxl2/default.px4board

@@ -3,5 +3,3 @@ CONFIG_BOARD_LINUX=y
 CONFIG_BOARD_TOOLCHAIN="aarch64-linux-gnu"
 CONFIG_MODULES_MUORB_APPS=y
 CONFIG_SYSTEMCMDS_PERF=y
-CONFIG_SYSTEMCMDS_UORB=y
-CONFIG_ORB_COMMUNICATOR=y

boards/modalai/voxl2/install-voxl.sh

@@ -1,19 +1,19 @@
 #!/bin/bash
 
-# Push slpi image to voxl2
-adb push build/modalai_voxl2-slpi_default/platforms/qurt/libpx4.so /usr/lib/rfsa/adsp
+# Push qurt image to voxl2
+adb push build/modalai_voxl2_qurt/platforms/qurt/libpx4.so /usr/lib/rfsa/adsp
 
-# Push apps processor image to voxl2
+# Push posix image to voxl2
 adb push build/modalai_voxl2_default/bin/px4 /usr/bin
 
 # Push scripts to voxl2
 adb push build/modalai_voxl2_default/bin/px4-alias.sh /usr/bin
-adb push boards/modalai/voxl2/target/voxl-px4 /usr/bin
+adb push boards/modalai/voxl2/voxl-px4 /usr/bin
 adb shell chmod a+x /usr/bin/px4-alias.sh
 adb shell chmod a+x /usr/bin/voxl-px4
 
 # Push configuration file
 adb shell mkdir -p /etc/modalai
-adb push boards/modalai/voxl2/target/voxl-px4.config /etc/modalai
+adb push boards/modalai/voxl2/voxl-px4.config /etc/modalai
 
 adb shell sync

boards/modalai/voxl2/qurt.px4board

@@ -1,5 +1,3 @@
 CONFIG_PLATFORM_QURT=y
 CONFIG_BOARD_TOOLCHAIN="qurt"
 CONFIG_MODULES_MUORB_SLPI=y
-CONFIG_SYSTEMCMDS_UORB=y
-CONFIG_ORB_COMMUNICATOR=y

boards/modalai/voxl2/scripts/build-slpi.sh

@@ -1,11 +0,0 @@
-#!/bin/bash
-
-echo "*** Starting qurt slpi build ***"
-
-source /home/build-env.sh
-
-make modalai_voxl2-slpi
-
-cat build/modalai_voxl2-slpi_default/src/lib/version/build_git_version.h
-
-echo "*** End of qurt slpi build ***"

boards/modalai/voxl2/src/board_config.h

@@ -42,8 +42,10 @@
 #define BOARD_HAS_NO_RESET
 #define BOARD_HAS_NO_BOOTLOADER
 
-// Define this as empty since there are no I2C buses
-#define BOARD_I2C_BUS_CLOCK_INIT
+/*
+ * I2C buses
+ */
+#define PX4_NUMBER_I2C_BUSES    3
 
 #include <system_config.h>
 #include <px4_platform_common/board_common.h>

boards/px4/fmu-v5x/src/mtd.cpp

@@ -71,8 +71,8 @@ static const px4_mtd_entry_t base_eeprom = {
 	.npart = 2,
 	.partd = {
 		{
-			.type = MTD_MFT_VER,
-			.path = "/fs/mtd_mft_ver",
+			.type = MTD_MFT,
+			.path = "/fs/mtd_mft",
 			.nblocks = 248
 		},
 		{
@@ -86,17 +86,12 @@ static const px4_mtd_entry_t base_eeprom = {
 
 static const px4_mtd_entry_t imu_eeprom = {
 	.device = &i2c4,
-	.npart = 3,
+	.npart = 2,
 	.partd = {
 		{
 			.type = MTD_CALDATA,
 			.path = "/fs/mtd_caldata",
-			.nblocks = 240
-		},
-		{
-			.type = MTD_MFT_REV,
-			.path = "/fs/mtd_mft_rev",
-			.nblocks = 8
+			.nblocks = 248
 		},
 		{
 			.type = MTD_ID,

boards/px4/fmu-v6c/src/mtd.cpp

@@ -65,17 +65,12 @@ static const px4_mtd_entry_t fmum_fram = {
 
 static const px4_mtd_entry_t imu_eeprom = {
 	.device = &i2c4,
-	.npart = 3,
+	.npart = 2,
 	.partd = {
 		{
 			.type = MTD_CALDATA,
 			.path = "/fs/mtd_caldata",
-			.nblocks = 240
-		},
-		{
-			.type = MTD_MFT_REV,
-			.path = "/fs/mtd_mft_rev",
-			.nblocks = 8
+			.nblocks = 248
 		},
 		{
 			.type = MTD_ID,

boards/px4/fmu-v6x/src/mtd.cpp

@@ -71,8 +71,8 @@ static const px4_mtd_entry_t base_eeprom = {
 	.npart = 2,
 	.partd = {
 		{
-			.type = MTD_MFT_VER,
-			.path = "/fs/mtd_mft_ver",
+			.type = MTD_MFT,
+			.path = "/fs/mtd_mft",
 			.nblocks = 248
 		},
 		{
@@ -86,17 +86,12 @@ static const px4_mtd_entry_t base_eeprom = {
 
 static const px4_mtd_entry_t imu_eeprom = {
 	.device = &i2c4,
-	.npart = 3,
+	.npart = 2,
 	.partd = {
 		{
 			.type = MTD_CALDATA,
 			.path = "/fs/mtd_caldata",
-			.nblocks = 240
-		},
-		{
-			.type = MTD_MFT_REV,
-			.path = "/fs/mtd_mft_rev",
-			.nblocks = 8
+			.nblocks = 248
 		},
 		{
 			.type = MTD_ID,

boards/sky-drones/smartap-airlink/src/mtd.cpp

@@ -66,8 +66,8 @@ static const px4_mtd_entry_t base_eeprom = {
 	.npart = 2,
 	.partd = {
 		{
-			.type = MTD_MFT_VER,
-			.path = "/fs/mtd_mft_ver",
+			.type = MTD_MFT,
+			.path = "/fs/mtd_mft",
 			.nblocks = 248
 		},
 		{

cmake/kconfig.cmake

@@ -34,7 +34,7 @@ if(EXISTS ${BOARD_DEFCONFIG})
     # Depend on BOARD_DEFCONFIG so that we reconfigure on config change
     set_property(DIRECTORY APPEND PROPERTY CMAKE_CONFIGURE_DEPENDS ${BOARD_DEFCONFIG})
 
-    if(${LABEL} MATCHES "default" OR ${LABEL} MATCHES "recovery" OR ${LABEL} MATCHES "bootloader" OR ${LABEL} MATCHES "canbootloader")
+    if(${LABEL} MATCHES "default" OR ${LABEL} MATCHES "qurt" OR ${LABEL} MATCHES "recovery" OR ${LABEL} MATCHES "bootloader" OR ${LABEL} MATCHES "canbootloader")
         # Generate boardconfig from saved defconfig
         execute_process(COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS}
                         ${DEFCONFIG_PATH} ${BOARD_DEFCONFIG}
@@ -228,6 +228,10 @@ if(EXISTS ${BOARD_DEFCONFIG})
 
         # platform-specific include path
         include_directories(${PX4_SOURCE_DIR}/platforms/${PX4_PLATFORM}/src/px4/common/include)
+
+        if(PLATFORM STREQUAL "qurt")
+            include(${PX4_SOURCE_DIR}/boards/modalai/voxl2/cmake/voxl2_qurt.cmake)
+        endif()
     endif()
 
 	if(ARCHITECTURE)

cmake/px4_add_module.cmake

@@ -170,9 +170,6 @@ function(px4_add_module)
 		if (${PX4_PLATFORM} STREQUAL "nuttx" AND NOT CONFIG_BUILD_FLAT AND KERNEL)
 			target_link_libraries(${MODULE} PRIVATE kernel_parameters_interface px4_kernel_layer uORB_kernel)
 			set_property(GLOBAL APPEND PROPERTY PX4_KERNEL_MODULE_LIBRARIES ${MODULE})
-		elseif(${PX4_PLATFORM} STREQUAL "qurt")
-			target_link_libraries(${MODULE} PRIVATE px4_layer uORB)
-			set_property(GLOBAL APPEND PROPERTY PX4_MODULE_LIBRARIES ${MODULE})
 		else()
 			target_link_libraries(${MODULE} PRIVATE parameters_interface px4_layer uORB)
 			set_property(GLOBAL APPEND PROPERTY PX4_MODULE_LIBRARIES ${MODULE})

msg/CMakeLists.txt

@@ -186,6 +186,7 @@ set(msg_files
 	UwbGrid.msg
 	VehicleAcceleration.msg
 	VehicleAirData.msg
+	VehicleAngularAcceleration.msg
 	VehicleAngularAccelerationSetpoint.msg
 	VehicleAngularVelocity.msg
 	VehicleAttitude.msg

msg/CameraCapture.msg

@@ -5,5 +5,5 @@ float64 lat					# Latitude in degrees (WGS84)
 float64 lon					# Longitude in degrees (WGS84)
 float32 alt					# Altitude (AMSL)
 float32 ground_distance			# Altitude above ground (meters)
-float32[4] q					# Attitude of the camera relative to NED earth-fixed frame when using a gimbal, otherwise vehicle attitude
+float32[4] q					# Attitude of the camera, zero rotation is facing towards front of vehicle
 int8 result					# 1 for success, 0 for failure, -1 if camera does not provide feedback

msg/VehicleAngularAcceleration.msg

@@ -0,0 +1,5 @@
+
+uint64 timestamp        # time since system start (microseconds)
+uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds)
+
+float32[3] xyz          # angular acceleration about the FRD body frame XYZ-axis in rad/s^2

msg/VehicleAngularVelocity.msg

@@ -1,9 +1,7 @@
 
-uint64 timestamp          # time since system start (microseconds)
-uint64 timestamp_sample   # timestamp of the data sample on which this message is based (microseconds)
+uint64 timestamp        # time since system start (microseconds)
+uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds)
 
-float32[3] xyz		  # Bias corrected angular velocity about the FRD body frame XYZ-axis in rad/s
-
-float32[3] xyz_derivative # angular acceleration about the FRD body frame XYZ-axis in rad/s^2
+float32[3] xyz		# Bias corrected angular velocity about the FRD body frame XYZ-axis in rad/s
 
 # TOPICS vehicle_angular_velocity vehicle_angular_velocity_groundtruth

platforms/common/CMakeLists.txt

@@ -33,7 +33,7 @@
 
 set(SRCS)
 
-if(NOT "${PX4_PLATFORM}" MATCHES "qurt" AND NOT "${PX4_BOARD}" MATCHES "io-v2" AND NOT "${PX4_BOARD_LABEL}" MATCHES "bootloader")
+if(NOT "${PX4_BOARD}" MATCHES "io-v2" AND NOT "${PX4_BOARD_LABEL}" MATCHES "bootloader")
 	list(APPEND SRCS
 		px4_log.cpp
 		px4_log_history.cpp

platforms/common/Kconfig

@@ -1 +0,0 @@
-rsource "*/Kconfig"

platforms/common/include/px4_platform_common/mtd_manifest.h

@@ -37,13 +37,12 @@ typedef enum  {
 	MTD_PARAMETERS  = 1,
 	MTD_WAYPOINTS   = 2,
 	MTD_CALDATA     = 3,
-	MTD_MFT_VER     = 4,
-	MTD_MFT_REV     = 5,
-	MTD_ID          = 6,
-	MTD_NET         = 7
+	MTD_MFT         = 4,
+	MTD_ID          = 5,
+	MTD_NET         = 6,
 } px4_mtd_types_t;
-#define PX4_MFT_MTD_TYPES  {MTD_PARAMETERS, MTD_WAYPOINTS, MTD_CALDATA, MTD_MFT_VER, MTD_MFT_REV, MTD_ID, MTD_NET}
-#define PX4_MFT_MTD_STR_TYPES  {"MTD_PARAMETERS", "MTD_WAYPOINTS", "MTD_CALDATA", "MTD_MFT_VER", "MTD_MFT_REV", "MTD_ID", "MTD_NET"}
+#define PX4_MFT_MTD_TYPES  {MTD_PARAMETERS, MTD_WAYPOINTS, MTD_CALDATA, MTD_MFT, MTD_ID, MTD_NET}
+#define PX4_MFT_MTD_STR_TYPES  {"MTD_PARAMETERS", "MTD_WAYPOINTS", "MTD_CALDATA", "MTD_MFT", "MTD_ID", "MTD_NET"}
 
 typedef struct  {
 	const px4_mtd_types_t   type;

platforms/common/uORB/Kconfig

@@ -1,5 +0,0 @@
-menuconfig ORB_COMMUNICATOR
-	bool "orb communicator"
-	default n
-	---help---
-		Enable support for the uorb communicator for distributed platforms

platforms/common/uORB/uORBDeviceMaster.cpp

@@ -36,6 +36,10 @@
 #include "uORBManager.hpp"
 #include "uORBUtils.hpp"
 
+#ifdef ORB_COMMUNICATOR
+#include "uORBCommunicator.hpp"
+#endif /* ORB_COMMUNICATOR */
+
 #include <px4_platform_common/sem.hpp>
 #include <systemlib/px4_macros.h>
 

platforms/common/uORB/uORBDeviceNode.cpp

@@ -38,9 +38,9 @@
 
 #include "SubscriptionCallback.hpp"
 
-#ifdef CONFIG_ORB_COMMUNICATOR
+#ifdef ORB_COMMUNICATOR
 #include "uORBCommunicator.hpp"
-#endif /* CONFIG_ORB_COMMUNICATOR */
+#endif /* ORB_COMMUNICATOR */
 
 #if defined(__PX4_NUTTX)
 #include <nuttx/mm/mm.h>
@@ -304,7 +304,7 @@ uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const v
 		return PX4_ERROR;
 	}
 
-#ifdef CONFIG_ORB_COMMUNICATOR
+#ifdef ORB_COMMUNICATOR
 	/*
 	 * if the write is successful, send the data over the Multi-ORB link
 	 */
@@ -317,7 +317,7 @@ uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const v
 		}
 	}
 
-#endif /* CONFIG_ORB_COMMUNICATOR */
+#endif /* ORB_COMMUNICATOR */
 
 	return PX4_OK;
 }
@@ -346,7 +346,7 @@ int uORB::DeviceNode::unadvertise(orb_advert_t handle)
 	return PX4_OK;
 }
 
-#ifdef CONFIG_ORB_COMMUNICATOR
+#ifdef ORB_COMMUNICATOR
 int16_t uORB::DeviceNode::topic_advertised(const orb_metadata *meta)
 {
 	uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
@@ -357,7 +357,19 @@ int16_t uORB::DeviceNode::topic_advertised(const orb_metadata *meta)
 
 	return -1;
 }
-#endif /* CONFIG_ORB_COMMUNICATOR */
+
+/*
+//TODO: Check if we need this since we only unadvertise when things all shutdown and it doesn't actually remove the device
+int16_t uORB::DeviceNode::topic_unadvertised(const orb_metadata *meta)
+{
+	uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
+	if (ch != nullptr && meta != nullptr) {
+		return ch->topic_unadvertised(meta->o_name);
+	}
+	return -1;
+}
+*/
+#endif /* ORB_COMMUNICATOR */
 
 px4_pollevent_t
 uORB::DeviceNode::poll_state(cdev::file_t *filp)
@@ -401,7 +413,7 @@ void uORB::DeviceNode::add_internal_subscriber()
 	lock();
 	_subscriber_count++;
 
-#ifdef CONFIG_ORB_COMMUNICATOR
+#ifdef ORB_COMMUNICATOR
 	uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
 
 	if (ch != nullptr && _subscriber_count > 0) {
@@ -409,7 +421,7 @@ void uORB::DeviceNode::add_internal_subscriber()
 		ch->add_subscription(_meta->o_name, 1);
 
 	} else
-#endif /* CONFIG_ORB_COMMUNICATOR */
+#endif /* ORB_COMMUNICATOR */
 
 	{
 		unlock();
@@ -421,7 +433,7 @@ void uORB::DeviceNode::remove_internal_subscriber()
 	lock();
 	_subscriber_count--;
 
-#ifdef CONFIG_ORB_COMMUNICATOR
+#ifdef ORB_COMMUNICATOR
 	uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
 
 	if (ch != nullptr && _subscriber_count == 0) {
@@ -429,13 +441,13 @@ void uORB::DeviceNode::remove_internal_subscriber()
 		ch->remove_subscription(_meta->o_name);
 
 	} else
-#endif /* CONFIG_ORB_COMMUNICATOR */
+#endif /* ORB_COMMUNICATOR */
 	{
 		unlock();
 	}
 }
 
-#ifdef CONFIG_ORB_COMMUNICATOR
+#ifdef ORB_COMMUNICATOR
 int16_t uORB::DeviceNode::process_add_subscription(int32_t rateInHz)
 {
 	// if there is already data in the node, send this out to
@@ -478,7 +490,7 @@ int16_t uORB::DeviceNode::process_received_message(int32_t length, uint8_t *data
 
 	return PX4_OK;
 }
-#endif /* CONFIG_ORB_COMMUNICATOR */
+#endif /* ORB_COMMUNICATOR */
 
 int uORB::DeviceNode::update_queue_size(unsigned int queue_size)
 {

platforms/common/uORB/uORBDeviceNode.hpp

@@ -41,7 +41,6 @@
 #include <containers/IntrusiveSortedList.hpp>
 #include <containers/List.hpp>
 #include <px4_platform_common/atomic.h>
-#include <px4_platform_common/px4_config.h>
 
 namespace uORB
 {
@@ -123,8 +122,9 @@ public:
 
 	static int        unadvertise(orb_advert_t handle);
 
-#ifdef CONFIG_ORB_COMMUNICATOR
+#ifdef ORB_COMMUNICATOR
 	static int16_t topic_advertised(const orb_metadata *meta);
+	//static int16_t topic_unadvertised(const orb_metadata *meta);
 
 	/**
 	 * processes a request for add subscription from remote
@@ -145,7 +145,7 @@ public:
 	 * processed the received data message from remote.
 	 */
 	int16_t process_received_message(int32_t length, uint8_t *data);
-#endif /* CONFIG_ORB_COMMUNICATOR */
+#endif /* ORB_COMMUNICATOR */
 
 	/**
 	  * Add the subscriber to the node's list of subscriber.  If there is

platforms/common/uORB/uORBManager.cpp

@@ -48,10 +48,6 @@
 #include "uORBUtils.hpp"
 #include "uORBManager.hpp"
 
-#ifdef CONFIG_ORB_COMMUNICATOR
-pthread_mutex_t uORB::Manager::_communicator_mutex = PTHREAD_MUTEX_INITIALIZER;
-#endif
-
 uORB::Manager *uORB::Manager::_Instance = nullptr;
 
 bool uORB::Manager::initialize()
@@ -262,7 +258,7 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
 		}
 	}
 
-#ifdef CONFIG_ORB_COMMUNICATOR
+#ifdef ORB_COMMUNICATOR
 
 	/*
 	 * Generate the path to the node and try to open it.
@@ -304,7 +300,7 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
 		}
 	}
 
-#endif /* CONFIG_ORB_COMMUNICATOR */
+#endif /* ORB_COMMUNICATOR */
 
 	return ret;
 }
@@ -364,10 +360,10 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta,
 		return nullptr;
 	}
 
-#ifdef CONFIG_ORB_COMMUNICATOR
+#ifdef ORB_COMMUNICATOR
 	// For remote systems call over and inform them
 	uORB::DeviceNode::topic_advertised(meta);
-#endif /* CONFIG_ORB_COMMUNICATOR */
+#endif /* ORB_COMMUNICATOR */
 
 	/* the advertiser may perform an initial publish to initialise the object */
 	if (data != nullptr) {
@@ -615,7 +611,7 @@ int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, i
 	return fd;
 }
 
-#ifdef CONFIG_ORB_COMMUNICATOR
+#ifdef ORB_COMMUNICATOR
 void uORB::Manager::set_uorb_communicator(uORBCommunicator::IChannel *channel)
 {
 	_comm_channel = channel;
@@ -627,53 +623,18 @@ void uORB::Manager::set_uorb_communicator(uORBCommunicator::IChannel *channel)
 
 uORBCommunicator::IChannel *uORB::Manager::get_uorb_communicator()
 {
-	pthread_mutex_lock(&_communicator_mutex);
-	uORBCommunicator::IChannel *temp = _comm_channel;
-	pthread_mutex_unlock(&_communicator_mutex);
-
-	return temp;
+	return _comm_channel;
 }
 
 int16_t uORB::Manager::process_remote_topic(const char *topic_name, bool isAdvertisement)
 {
-	PX4_DEBUG("entering process_remote_topic: name: %s", topic_name);
-
 	int16_t rc = 0;
 
-	char nodepath[orb_maxpath];
-	int ret = uORB::Utils::node_mkpath(nodepath, topic_name);
-	DeviceMaster *device_master = get_device_master();
-
-	if (ret == OK && device_master && isAdvertisement) {
-		uORB::DeviceNode *node = device_master->getDeviceNode(nodepath);
-
-		if (node) {
-			node->mark_as_advertised();
-			_remote_topics.insert(topic_name);
-			return rc;
-		}
-	}
-
-	// Didn't find a node so we need to create it via an advertisement
-	const struct orb_metadata *const *topic_list = orb_get_topics();
-	orb_id_t topic_ptr = nullptr;
-
-	for (size_t i = 0; i < orb_topics_count(); i++) {
-		if (strcmp(topic_list[i]->o_name, topic_name) == 0) {
-			topic_ptr = topic_list[i];
-			break;
-		}
-	}
-
-	if (topic_ptr) {
-		PX4_INFO("Advertising remote topic %s", topic_name);
+	if (isAdvertisement) {
 		_remote_topics.insert(topic_name);
-		orb_advertise(topic_ptr, nullptr);
 
 	} else {
-		PX4_INFO("process_remote_topic meta not found for %s\n", topic_name);
 		_remote_topics.erase(topic_name);
-		rc = -1;
 	}
 
 	return rc;
@@ -762,7 +723,7 @@ bool uORB::Manager::is_remote_subscriber_present(const char *messageName)
 	return _remote_subscriber_topics.find(messageName);
 }
 
-#endif /* CONFIG_ORB_COMMUNICATOR */
+#endif /* ORB_COMMUNICATOR */
 
 #ifdef ORB_USE_PUBLISHER_RULES
 

platforms/common/uORB/uORBManager.hpp

@@ -40,12 +40,11 @@
 
 #include <uORB/topics/uORBTopics.hpp> // For ORB_ID enum
 #include <stdint.h>
-#include <px4_platform_common/px4_config.h>
 
-#ifdef CONFIG_ORB_COMMUNICATOR
+#ifdef ORB_COMMUNICATOR
 #include "ORBSet.hpp"
 #include "uORBCommunicator.hpp"
-#endif /* CONFIG_ORB_COMMUNICATOR */
+#endif /* ORB_COMMUNICATOR */
 
 namespace uORB
 {
@@ -160,9 +159,9 @@ typedef enum {
  * uORB Api's.
  */
 class uORB::Manager
-#ifdef CONFIG_ORB_COMMUNICATOR
+#ifdef ORB_COMMUNICATOR
 	: public uORBCommunicator::IChannelRxHandler
-#endif /* CONFIG_ORB_COMMUNICATOR */
+#endif /* ORB_COMMUNICATOR */
 {
 public:
 	// public interfaces for this class.
@@ -465,7 +464,7 @@ public:
 	static bool is_advertised(const void *node_handle);
 #endif
 
-#ifdef CONFIG_ORB_COMMUNICATOR
+#ifdef ORB_COMMUNICATOR
 	/**
 	 * Method to set the uORBCommunicator::IChannel instance.
 	 * @param comm_channel
@@ -486,7 +485,7 @@ public:
 	 * for a given topic
 	 */
 	bool is_remote_subscriber_present(const char *messageName);
-#endif /* CONFIG_ORB_COMMUNICATOR */
+#endif /* ORB_COMMUNICATOR */
 
 private: // class methods
 
@@ -501,14 +500,13 @@ private: // class methods
 private: // data members
 	static Manager *_Instance;
 
-#ifdef CONFIG_ORB_COMMUNICATOR
+#ifdef ORB_COMMUNICATOR
 	// the communicator channel instance.
 	uORBCommunicator::IChannel *_comm_channel{nullptr};
-	static pthread_mutex_t _communicator_mutex;
 
 	ORBSet _remote_subscriber_topics;
 	ORBSet _remote_topics;
-#endif /* CONFIG_ORB_COMMUNICATOR */
+#endif /* ORB_COMMUNICATOR */
 
 	DeviceMaster *_device_master{nullptr};
 
@@ -516,7 +514,7 @@ private: //class methods
 	Manager();
 	virtual ~Manager();
 
-#ifdef CONFIG_ORB_COMMUNICATOR
+#ifdef ORB_COMMUNICATOR
 	/**
 	 * Interface to process a received topic from remote.
 	 * @param topic_name
@@ -572,7 +570,7 @@ private: //class methods
 	 *  otherwise = failure.
 	 */
 	virtual int16_t process_received_message(const char *messageName, int32_t length, uint8_t *data);
-#endif /* CONFIG_ORB_COMMUNICATOR */
+#endif /* ORB_COMMUNICATOR */
 
 #ifdef ORB_USE_PUBLISHER_RULES
 

platforms/nuttx/src/px4/common/cdc_acm_check.cpp

@@ -35,7 +35,6 @@
 
 #if defined(CONFIG_SYSTEM_CDCACM)
 __BEGIN_DECLS
-#include <board_config.h>
 #include <arch/board/board.h>
 #include <syslog.h>
 #include <nuttx/wqueue.h>
@@ -43,7 +42,6 @@ __BEGIN_DECLS
 
 #include <termios.h>
 #include <sys/ioctl.h>
-#include <fcntl.h>
 
 extern int sercon_main(int c, char **argv);
 extern int serdis_main(int c, char **argv);

platforms/nuttx/src/px4/common/include/px4_platform/board_hw_eeprom_rev_ver.h

@@ -32,10 +32,8 @@
  ****************************************************************************/
 #pragma once
 
-#include <errno.h>
-
-#define HW_ID_EEPROM 			0x7		//!< Get hw_info from EEPROM
-#define HW_EEPROM_ID_MIN		0x10	//!< Minimum supported id (version/revision)
+#define HW_VERSION_EEPROM 		0x7		//!< Get hw_info from EEPROM
+#define HW_EEPROM_VERSION_MIN	0x10	//!< Minimum supported version
 
 #pragma pack(push, 1)
 
@@ -45,13 +43,13 @@ typedef struct {
 
 typedef struct {
 	mtd_mft_t version;
-	uint16_t hw_extended_id; //<! HW version for MTD_MFT_VER, HW revision for MTD_MFT_REV
+	uint16_t hw_extended_ver;
 	uint16_t crc;
 } mtd_mft_v0_t;
 
 typedef struct {
-	mtd_mft_t version;
-	uint16_t hw_extended_id;
+	mtd_mft_t  version;
+	uint16_t hw_extended_ver;
 	//{device tree overlay}
 	uint16_t crc;
 } mtd_mft_v1_t;

platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c

@@ -445,36 +445,22 @@ __EXPORT int board_get_hw_revision()
 
 int board_determine_hw_info()
 {
+	// MFT supported?
+	const char *path;
+	int rvmft = px4_mtd_query("MTD_MFT", NULL, &path);
 
 	// Read ADC jumpering hw_info
 	int rv = determine_hw_info(&hw_revision, &hw_version);
 
 	if (rv == OK) {
 
-		// MFT supported?
-		const char *path;
-		int rvmft = px4_mtd_query("MTD_MFT_VER", NULL, &path);
-
-		if (rvmft == OK && path != NULL && hw_version == HW_ID_EEPROM) {
+		if (rvmft == OK && path != NULL && hw_version == HW_VERSION_EEPROM) {
 
 			mtd_mft_v0_t mtd_mft = {MTD_MFT_v0};
 			rv = board_get_eeprom_hw_info(path, (mtd_mft_t *)&mtd_mft);
 
 			if (rv == OK) {
-				hw_version = mtd_mft.hw_extended_id;
-			}
-		}
-
-		path = NULL;
-		rvmft = px4_mtd_query("MTD_MFT_REV", NULL, &path);
-
-		if (rvmft == OK && path != NULL && hw_revision == HW_ID_EEPROM) {
-
-			mtd_mft_v0_t mtd_mft = {MTD_MFT_v0};
-			rv = board_get_eeprom_hw_info(path, (mtd_mft_t *)&mtd_mft);
-
-			if (rv == OK) {
-				hw_revision = mtd_mft.hw_extended_id;
+				hw_version = mtd_mft.hw_extended_ver;
 			}
 		}
 	}
@@ -509,14 +495,14 @@ int board_set_eeprom_hw_info(const char *path, mtd_mft_t *mtd_mft_unk)
 
 	// Later this will be a demux on type
 	if (mtd_mft_unk->id != MTD_MFT_v0) {
-		printf("Version is: %d, Only mft version %d is supported\n", mtd_mft_unk->id, MTD_MFT_v0);
+		printf("Verson is: %d, Only mft version %d is supported\n", mtd_mft_unk->id, MTD_MFT_v0);
 		return -EINVAL;
 	}
 
 	mtd_mft_v0_t *mtd_mft = (mtd_mft_v0_t *)mtd_mft_unk;
 
-	if (mtd_mft->hw_extended_id < HW_EEPROM_ID_MIN) {
-		printf("hardware version for EEPROM must be greater than %x\n", HW_EEPROM_ID_MIN);
+	if (mtd_mft->hw_extended_ver < HW_EEPROM_VERSION_MIN) {
+		printf("hardware version for EEPROM must be greater than %x\n", HW_EEPROM_VERSION_MIN);
 		return -EINVAL;
 	}
 

platforms/posix/CMakeLists.txt

@@ -97,14 +97,18 @@ if(EXISTS "${PX4_BOARD_DIR}/cmake/upload.cmake")
 	include(${PX4_BOARD_DIR}/cmake/upload.cmake)
 endif()
 
-# board defined link libraries
-if(EXISTS "${PX4_BOARD_DIR}/cmake/link_libraries.cmake")
-	include(${PX4_BOARD_DIR}/cmake/link_libraries.cmake)
-endif()
 
 if("${PX4_BOARD}" MATCHES "beaglebone_blue")
 	target_link_libraries(px4 PRIVATE robotics_cape)
 
+elseif("${PX4_BOARD}" MATCHES "modalai_voxl2")
+	# libfc_sensor.so is provided in the Docker build environment
+	target_link_libraries(px4 PRIVATE
+			/home/libfc_sensor.so
+			px4_layer
+			${module_libraries}
+	)
+
 elseif("${PX4_BOARD}" MATCHES "emlid_navio2")
 	target_link_libraries(px4 PRIVATE atomic)
 

platforms/qurt/CMakeLists.txt

@@ -31,18 +31,12 @@
 #
 ############################################################################
 
-get_property(module_libraries GLOBAL PROPERTY PX4_MODULE_LIBRARIES)
+set(module_libraries modules__muorb__slpi)
 
-include_directories(
-	${CMAKE_CURRENT_BINARY_DIR}
-	)
-
-add_library(px4 SHARED
-	${PX4_SOURCE_DIR}/platforms/qurt/unresolved_symbols.c
-	)
-
-target_link_libraries(px4
-	modules__muorb__slpi
-	${module_libraries}
-	px4_layer
+QURT_LIB(LIB_NAME px4
+	SOURCES
+		${PX4_SOURCE_DIR}/platforms/qurt/unresolved_symbols.c
+	LINK_LIBS
+		${module_libraries}
+		px4_layer
 	)

platforms/qurt/cmake/px4_impl_os.cmake

@@ -31,27 +31,6 @@
 #
 ############################################################################
 
-if ("$ENV{HEXAGON_SDK_ROOT}" STREQUAL "")
-	message(FATAL_ERROR "Enviroment variable HEXAGON_SDK_ROOT must be set")
-else()
-	set(HEXAGON_SDK_ROOT $ENV{HEXAGON_SDK_ROOT})
-endif()
-
-if ("$ENV{HEXAGON_TOOLS_ROOT}" STREQUAL "")
-	message(FATAL_ERROR "Environment variable HEXAGON_TOOLS_ROOT must be set")
-else()
-	set(HEXAGON_TOOLS_ROOT $ENV{HEXAGON_TOOLS_ROOT})
-endif()
-
-include(px4_git)
-
-include(Toolchain-qurt)
-include(qurt_reqs)
-
-include_directories(${HEXAGON_SDK_INCLUDES})
-
-set(DISABLE_PARAMS_MODULE_SCOPING TRUE)
-
 #=============================================================================
 #
 #	Defined functions in this file

platforms/qurt/cmake/qurt_reqs.cmake

@@ -176,3 +176,46 @@ list2string(CMAKE_EXE_LINKER_FLAGS
 	)
 
 include (CMakeParseArguments)
+
+# Process DSP files
+function (QURT_LIB)
+	set(options)
+	set(oneValueArgs LIB_NAME)
+	set(multiValueArgs SOURCES LINK_LIBS INCS FLAGS)
+	cmake_parse_arguments(QURT_LIB "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN} )
+
+	include_directories(
+		${CMAKE_CURRENT_BINARY_DIR}
+		)
+
+	if ("${QURT_LIB_SOURCES}" STREQUAL "")
+		message(FATAL_ERROR "QURT_LIB called without SOURCES")
+	else()
+		# Build lib that is run on the DSP
+		add_library(${QURT_LIB_LIB_NAME} SHARED
+			${QURT_LIB_SOURCES}
+			)
+
+		if (NOT "${QURT_LIB_FLAGS}" STREQUAL "")
+			set_target_properties(${QURT_LIB_LIB_NAME} PROPERTIES COMPILE_FLAGS "${QURT_LIB_FLAGS}")
+		endif()
+
+		if (NOT "${QURT_LIB_INCS}" STREQUAL "")
+			target_include_directories(${QURT_LIB_LIB_NAME} PUBLIC ${QURT_LIB_INCS})
+		endif()
+
+		target_link_libraries(${QURT_LIB_LIB_NAME}
+			${QURT_LIB_LINK_LIBS}
+			)
+	endif()
+
+	set(DSPLIB_TARGET_PATH "/usr/lib/rfsa/adsp/")
+
+	# Add a rule to load the files onto the target that run in the DSP
+	add_custom_target(lib${QURT_LIB_LIB_NAME}-load
+		DEPENDS ${QURT_LIB_LIB_NAME}
+		COMMAND adb wait-for-device
+		COMMAND adb push lib${QURT_LIB_LIB_NAME}.so ${DSPLIB_TARGET_PATH}
+		COMMAND echo "Pushed lib${QURT_LIB_LIB_NAME}.so and dependencies to ${DSPLIB_TARGET_PATH}"
+		)
+endfunction()

platforms/qurt/include/qurt_log.h

@@ -35,7 +35,6 @@
 #include <stdarg.h>
 #include <stdio.h>
 #include <stdint.h>
-#include <px4_platform_common/defines.h>
 
 __BEGIN_DECLS
 

platforms/qurt/include/qurt_reqs.h

@@ -36,13 +36,13 @@
 
 #pragma once
 
-#include <pthread.h>
-#include <visibility.h>
-
-__BEGIN_DECLS
+#ifdef __cplusplus
+extern "C" {
+#endif
 
 typedef unsigned long useconds_t;
 int usleep(useconds_t usec);
-int pthread_setname_np(pthread_t __target_thread, const char *__name);
 
-__END_DECLS
+#ifdef __cplusplus
+}
+#endif

platforms/qurt/src/px4/CMakeLists.txt

@@ -35,7 +35,6 @@
 set(QURT_LAYER_SRCS
 	drv_hrt.cpp
 	tasks.cpp
-	px4_qurt_impl.cpp
 	)
 
 add_library(px4_layer

platforms/qurt/src/px4/px4_qurt_impl.cpp

@@ -1,39 +0,0 @@
-/****************************************************************************
- *
- * Copyright (C) 2022 ModalAI, Inc. 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.
- *
- ****************************************************************************/
-#include <px4_platform_common/defines.h>
-
-__BEGIN_DECLS
-
-long PX4_TICKS_PER_SEC = 1000L;
-
-__END_DECLS

src/drivers/actuators/modalai_esc/modalai_esc_params.c

@@ -180,6 +180,16 @@ PARAM_DEFINE_FLOAT(UART_ESC_T_MINF, 0.15);
  */
 PARAM_DEFINE_INT32(UART_ESC_T_EXPO, 35);
 
+/**
+ * UART ESC Turtle Mode Yaw Reversal
+ *
+ * @group UART ESC
+ * @min 0
+ * @max 1
+ * @decimal 10
+ * @increment 1
+ */
+PARAM_DEFINE_INT32(UART_ESC_T_YAWR, 0);
 /**
  * UART ESC Turtle Mode Cosphi
  *

src/drivers/drv_hrt.h

@@ -149,8 +149,8 @@ static inline hrt_abstime ts_to_abstime(const struct timespec *ts)
  */
 static inline void abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
 {
-	ts->tv_sec = (typeof(ts->tv_sec))(abstime / 1000000);
-	abstime -= (hrt_abstime)(ts->tv_sec) * 1000000;
+	ts->tv_sec = (time_t)abstime / 1000000;
+	abstime -= (hrt_abstime)(ts->tv_sec * 1000000);
 	ts->tv_nsec = (typeof(ts->tv_nsec))(abstime * 1000);
 }
 

src/drivers/gps/gps.cpp

@@ -535,12 +535,11 @@ void GPS::handleInjectDataTopic()
 
 	bool updated = false;
 
-	// Limit maximum number of GPS injections to 8 since usually
+	// Limit maximum number of GPS injections to 6 since usually
 	// GPS injections should consist of 1-4 packets (GPS, Glonass, BeiDou, Galileo).
-	// Looking at 8 packets thus guarantees, that at least a full injection
+	// Looking at 6 packets thus guarantees, that at least a full injection
 	// data set is evaluated.
-	// Moving Base reuires a higher rate, so we allow up to 8 packets.
-	const size_t max_num_injections = gps_inject_data_s::ORB_QUEUE_LENGTH;
+	const size_t max_num_injections = 6;
 	size_t num_injections = 0;
 
 	do {

src/drivers/rc/crsf_rc/CrsfRc.cpp

@@ -37,7 +37,6 @@
 
 #include <poll.h>
 #include <termios.h>
-#include <fcntl.h>
 
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/vehicle_attitude.h>

src/drivers/safety_button/SafetyButton.cpp

@@ -31,7 +31,6 @@
  *
  ****************************************************************************/
 
-#include <board_config.h>
 #include "SafetyButton.hpp"
 
 #ifndef GPIO_BTN_SAFETY

src/drivers/uavcannode/UavcanNode.cpp

@@ -316,10 +316,10 @@ int UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events
 	_subscriber_list.add(new BeepCommand(_node));
 	_subscriber_list.add(new LightsCommand(_node));
 
-	int32_t cannode_sub_mbd = 0;
-	param_get(param_find("CANNODE_SUB_MBD"), &cannode_sub_mbd);
+	int32_t cannode_sub_mdb = 0;
+	param_get(param_find("CANNODE_SUB_MDB"), &cannode_sub_mdb);
 
-	if (cannode_sub_mbd == 1) {
+	if (cannode_sub_mdb == 1) {
 		_subscriber_list.add(new MovingBaselineData(_node));
 	}
 

src/lib/avoidance/ObstacleAvoidance.cpp

@@ -67,10 +67,10 @@ void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel
 	const auto &wp_msg = _sub_vehicle_trajectory_waypoint.get();
 	const auto &bezier_msg = _sub_vehicle_trajectory_bezier.get();
 
-	const bool wp_msg_timeout = hrt_elapsed_time((hrt_abstime *)&wp_msg.timestamp) > TRAJECTORY_STREAM_TIMEOUT_US;
-	const bool bezier_msg_timeout = hrt_elapsed_time((hrt_abstime *)&bezier_msg.timestamp) > hrt_abstime(
-						bezier_msg.control_points[bezier_msg.bezier_order - 1].delta * 1e6f);
-	const bool avoidance_data_timeout = wp_msg_timeout && bezier_msg_timeout;
+	const bool avoidance_data_timeout =
+		hrt_elapsed_time((hrt_abstime *)&wp_msg.timestamp) > TRAJECTORY_STREAM_TIMEOUT_US &&
+		hrt_elapsed_time((hrt_abstime *)&bezier_msg.timestamp) > hrt_abstime(bezier_msg.control_points[bezier_msg.bezier_order -
+							1].delta * 1e6f);
 
 	const bool avoidance_point_valid = wp_msg.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid;
 	const bool avoidance_bezier_valid = bezier_msg.bezier_order > 0;
@@ -86,12 +86,8 @@ void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel
 	}
 
 	if (avoidance_invalid) {
-		if (_avoidance_activated) {
-			// Invalid point received: deactivate
-			PX4_WARN("Obstacle Avoidance system failed, loitering");
-			_publishVehicleCmdDoLoiter();
-			_avoidance_activated = false;
-		}
+		PX4_WARN("Obstacle Avoidance system failed, loitering");
+		_publishVehicleCmdDoLoiter();
 
 		if (!_failsafe_position.isAllFinite()) {
 			// save vehicle position when entering failsafe
@@ -102,18 +98,13 @@ void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel
 		vel_sp.setNaN();
 		yaw_sp = NAN;
 		yaw_speed_sp = NAN;
-
-		// Do nothing further - wait until activation
 		return;
 
-	} else if (!_avoidance_activated) {
-		// First setpoint has been received: activate
-		PX4_INFO("Obstacle Avoidance system activated");
+	} else {
 		_failsafe_position.setNaN();
-		_avoidance_activated = true;
 	}
 
-	if (avoidance_point_valid && !wp_msg_timeout) {
+	if (avoidance_point_valid) {
 		const auto &point0 = wp_msg.waypoints[vehicle_trajectory_waypoint_s::POINT_0];
 		pos_sp = Vector3f(point0.position);
 		vel_sp = Vector3f(point0.velocity);
@@ -124,7 +115,8 @@ void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel
 			yaw_speed_sp = point0.yaw_speed;
 		}
 
-	} else if (avoidance_bezier_valid && !bezier_msg_timeout) {
+	} else if (avoidance_bezier_valid) {
+
 		float yaw = NAN, yaw_speed = NAN;
 		_generateBezierSetpoints(pos_sp, vel_sp, yaw, yaw_speed);
 

src/lib/avoidance/ObstacleAvoidance.hpp

@@ -130,8 +130,6 @@ protected:
 	matrix::Vector3f _position = {}; /**< current vehicle position */
 	matrix::Vector3f _failsafe_position = {}; /**< vehicle position when entered in failsafe */
 
-	bool _avoidance_activated{false}; /**< true after the first avoidance setpoint is received */
-
 	systemlib::Hysteresis _avoidance_point_not_valid_hysteresis{false}; /**< becomes true if the companion doesn't start sending valid setpoints */
 	systemlib::Hysteresis _no_progress_z_hysteresis{false}; /**< becomes true if the vehicle is not making progress towards the z component of the goal */
 

src/lib/parameters/param_translation.cpp

@@ -259,16 +259,5 @@ bool param_modify_on_import(bson_node_t node)
 		}
 	}
 
-	// 2022-11-11: translate VT_F_TRANS_THR/VT_PSHER_RMP_DT -> VT_PSHER_SLEW
-	{
-		if (strcmp("VT_PSHER_RMP_DT", node->name) == 0) {
-			strcpy(node->name, "VT_PSHER_SLEW");
-			double _param_vt_f_trans_thr = param_find("VT_F_TRANS_THR");
-			node->d = _param_vt_f_trans_thr / node->d;
-			PX4_INFO("copying %s -> %s", "VT_PSHER_RMP_DT", "VT_PSHER_SLEW");
-			return true;
-		}
-	}
-
 	return false;
 }

src/modules/angular_velocity_controller/AngularVelocityController.cpp

@@ -136,7 +136,6 @@ AngularVelocityController::Run()
 		_last_run = now;
 
 		const Vector3f angular_velocity{vehicle_angular_velocity.xyz};
-		_angular_acceleration = Vector3f(vehicle_angular_velocity.xyz_derivative);
 
 		/* check for updates in other topics */
 		_vehicle_status_sub.update(&_vehicle_status);
@@ -165,6 +164,13 @@ AngularVelocityController::Run()
 			}
 		}
 
+		// check angular acceleration topic
+		vehicle_angular_acceleration_s vehicle_angular_acceleration;
+
+		if (_vehicle_angular_acceleration_sub.update(&vehicle_angular_acceleration)) {
+			_angular_acceleration = Vector3f(vehicle_angular_acceleration.xyz);
+		}
+
 		// check rates setpoint topic
 		vehicle_rates_setpoint_s vehicle_rates_setpoint;
 

src/modules/angular_velocity_controller/AngularVelocityController.hpp

@@ -50,6 +50,7 @@
 #include <uORB/topics/hover_thrust_estimate.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/rate_ctrl_status.h>
+#include <uORB/topics/vehicle_angular_acceleration.h>
 #include <uORB/topics/vehicle_angular_acceleration_setpoint.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_control_mode.h>
@@ -104,6 +105,7 @@ private:
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
 	uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)};				/**< motor limits subscription */
+	uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)}; 	/**< vehicle angular acceleration subscription */
 	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};			/**< vehicle control mode subscription */
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};			/**< vehicle land detected subscription */
 	uORB::Subscription _vehicle_rates_setpoint_sub{ORB_ID(vehicle_rates_setpoint)};			/**< vehicle rates setpoint subscription */

src/modules/camera_feedback/CameraFeedback.cpp

@@ -33,8 +33,6 @@
 
 #include "CameraFeedback.hpp"
 
-using namespace time_literals;
-
 CameraFeedback::CameraFeedback() :
 	ModuleParams(nullptr),
 	WorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
@@ -114,39 +112,11 @@ CameraFeedback::Run()
 		}
 
 		// Fill attitude data
-		gimbal_device_attitude_status_s gimbal{};
-
-		if (_gimbal_sub.copy(&gimbal) && (hrt_elapsed_time(&gimbal.timestamp) < 1_s)) {
-			if (gimbal.device_flags & gimbal_device_attitude_status_s::DEVICE_FLAGS_YAW_LOCK) {
-				// Gimbal yaw angle is absolute angle relative to North
-				capture.q[0] = gimbal.q[0];
-				capture.q[1] = gimbal.q[1];
-				capture.q[2] = gimbal.q[2];
-				capture.q[3] = gimbal.q[3];
-
-			} else {
-				// Gimbal quaternion frame is in the Earth frame rotated so that the x-axis is pointing
-				// forward (yaw relative to vehicle). Get heading from the vehicle attitude and combine it
-				// with the gimbal orientation.
-				const matrix::Eulerf euler_vehicle(matrix::Quatf(att.q));
-				const matrix::Quatf q_heading(matrix::Eulerf(0.0f, 0.0f, euler_vehicle(2)));
-				matrix::Quatf q_gimbal(gimbal.q);
-				q_gimbal = q_heading * q_gimbal;
-
-				capture.q[0] = q_gimbal(0);
-				capture.q[1] = q_gimbal(1);
-				capture.q[2] = q_gimbal(2);
-				capture.q[3] = q_gimbal(3);
-			}
-
-		} else {
-			// No gimbal orientation, use vehicle attitude
-			capture.q[0] = att.q[0];
-			capture.q[1] = att.q[1];
-			capture.q[2] = att.q[2];
-			capture.q[3] = att.q[3];
-		}
-
+		// TODO : this needs to be rotated by camera orientation or set to gimbal orientation when available
+		capture.q[0] = att.q[0];
+		capture.q[1] = att.q[1];
+		capture.q[2] = att.q[2];
+		capture.q[3] = att.q[3];
 		capture.result = 1;
 
 		_capture_pub.publish(capture);

src/modules/camera_feedback/CameraFeedback.hpp

@@ -55,7 +55,6 @@
 #include <uORB/topics/camera_trigger.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/gimbal_device_attitude_status.h>
 
 class CameraFeedback : public ModuleBase<CameraFeedback>, public ModuleParams, public px4::WorkItem
 {
@@ -82,7 +81,6 @@ private:
 
 	uORB::Subscription	_gpos_sub{ORB_ID(vehicle_global_position)};
 	uORB::Subscription	_att_sub{ORB_ID(vehicle_attitude)};
-	uORB::Subscription	_gimbal_sub{ORB_ID(gimbal_device_attitude_status)};
 
 	uORB::Publication<camera_capture_s>	_capture_pub{ORB_ID(camera_capture)};
 

src/modules/commander/Commander.cpp

@@ -1862,22 +1862,17 @@ void Commander::checkForMissionUpdate()
 			}
 		}
 
+		// Transition mode to loiter or auto-mission after takeoff is completed.
 		if (_arm_state_machine.isArmed() && !_vehicle_land_detected.landed
+		    && (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF ||
+			_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF)
 		    && (mission_result.timestamp >= _vehicle_status.nav_state_timestamp)
 		    && mission_result.finished) {
 
-			if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF
-			    || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF) {
-				// Transition mode to loiter or auto-mission after takeoff is completed.
-				if ((_param_takeoff_finished_action.get() == 1) && auto_mission_available) {
-					_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION);
+			if ((_param_takeoff_finished_action.get() == 1) && auto_mission_available) {
+				_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION);
 
-				} else {
-					_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER);
-				}
-
-			} else if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) {
-				// Transition to loiter when the mission is cleared and/or finished, and we are still in mission mode.
+			} else {
 				_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER);
 			}
 		}

src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp

@@ -35,7 +35,8 @@
 
 HealthAndArmingChecks::HealthAndArmingChecks(ModuleParams *parent, vehicle_status_s &status)
 	: ModuleParams(parent),
-	  _context(status)
+	  _context(status),
+	  _reporter(_failsafe_flags)
 {
 	// Initialize mode requirements to invalid
 	_failsafe_flags.angular_velocity_invalid = true;

src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp

@@ -103,12 +103,12 @@ public:
 protected:
 	void updateParams() override;
 private:
-	failsafe_flags_s _failsafe_flags{};
-
 	Context _context;
-	Report _reporter{_failsafe_flags};
+	Report _reporter;
 	orb_advert_t _mavlink_log_pub{nullptr};
 
+	failsafe_flags_s _failsafe_flags{};
+
 	uORB::Publication<health_report_s> _health_report_pub{ORB_ID(health_report)};
 	uORB::Publication<failsafe_flags_s> _failsafe_flags_pub{ORB_ID(failsafe_flags)};
 

src/modules/commander/commander_params.c

@@ -142,6 +142,23 @@ PARAM_DEFINE_INT32(COM_HLDL_REG_T, 0);
  */
 PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5f);
 
+/**
+ * Delay between RC loss and configured reaction
+ *
+ * RC signal not updated -> still use data for COM_RC_LOSS_T seconds
+ * Consider RC signal lost -> wait COM_RCL_ACT_T seconds in Hold mode to regain signal
+ * React with failsafe action NAV_RCL_ACT
+ *
+ * A zero value disables the delay.
+ *
+ * @group Commander
+ * @unit s
+ * @min 0.0
+ * @max 25.0
+ * @decimal 3
+ */
+PARAM_DEFINE_FLOAT(COM_RCL_ACT_T, 15.0f);
+
 /**
  * Home position enabled
  *
@@ -207,8 +224,7 @@ PARAM_DEFINE_INT32(COM_RC_ARM_HYST, 1000);
  *
  * @group Commander
  * @unit s
- * @decimal 1
- * @increment 0.1
+ * @decimal 2
  */
 
 PARAM_DEFINE_FLOAT(COM_DISARM_LAND, 2.0f);
@@ -224,8 +240,7 @@ PARAM_DEFINE_FLOAT(COM_DISARM_LAND, 2.0f);
  *
  * @group Commander
  * @unit s
- * @decimal 1
- * @increment 0.1
+ * @decimal 2
  */
 PARAM_DEFINE_FLOAT(COM_DISARM_PRFLT, 10.0f);
 
@@ -262,6 +277,8 @@ PARAM_DEFINE_INT32(COM_ARM_SWISBTN, 0);
  * @value 0 Warning
  * @value 2 Land mode
  * @value 3 Return at critical level, land at emergency level
+ * @decimal 0
+ * @increment 1
  */
 PARAM_DEFINE_INT32(COM_LOW_BAT_ACT, 0);
 
@@ -295,6 +312,7 @@ PARAM_DEFINE_FLOAT(COM_FAIL_ACT_T, 5.f);
  * @value 0 Warning
  * @value 1 Return
  * @value 2 Land
+ * @decimal 0
  * @increment 1
  */
 PARAM_DEFINE_INT32(COM_IMB_PROP_ACT, 0);
@@ -1015,8 +1033,6 @@ PARAM_DEFINE_INT32(COM_ARM_HFLT_CHK, 1);
  * @group Commander
  * @min 0
  * @max 30
- * @decimal 1
- * @increment 0.1
  * @unit s
  */
 PARAM_DEFINE_FLOAT(COM_SPOOLUP_TIME, 1.0f);

src/modules/commander/failsafe/failsafe.cpp

@@ -467,11 +467,6 @@ FailsafeBase::Action Failsafe::checkModeFallback(const failsafe_flags_s &status_
 	// offboard signal
 	if (status_flags.offboard_control_signal_lost && (status_flags.mode_req_offboard_signal & (1u << user_intended_mode))) {
 		action = fromOffboardLossActParam(_param_com_obl_rc_act.get(), user_intended_mode);
-
-		// for this specific case, user_intended_mode is not modified, we shouldn't check additional fallbacks
-		if (action == Action::Disarm) {
-			return action;
-		}
 	}
 
 	// posctrl

src/modules/ekf2/CMakeLists.txt

@@ -92,7 +92,6 @@ px4_add_module(
 		EKF/EKFGSF_yaw.cpp
 		EKF/estimator_interface.cpp
 		EKF/ev_height_control.cpp
-		EKF/ev_vel_control.cpp
 		EKF/fake_height_control.cpp
 		EKF/fake_pos_control.cpp
 		EKF/gnss_height_control.cpp

src/modules/ekf2/EKF/CMakeLists.txt

@@ -43,7 +43,6 @@ add_library(ecl_EKF
 	EKFGSF_yaw.cpp
 	estimator_interface.cpp
 	ev_height_control.cpp
-	ev_vel_control.cpp
 	fake_height_control.cpp
 	fake_pos_control.cpp
 	gnss_height_control.cpp

src/modules/ekf2/EKF/common.h

@@ -66,23 +66,21 @@ using math::Utilities::sq;
 using math::Utilities::updateYawInRotMat;
 
 // maximum sensor intervals in usec
-static constexpr uint64_t BARO_MAX_INTERVAL     = 200e3;  ///< Maximum allowable time interval between pressure altitude measurements (uSec)
-static constexpr uint64_t EV_MAX_INTERVAL       = 200e3;  ///< Maximum allowable time interval between external vision system measurements (uSec)
-static constexpr uint64_t GNSS_MAX_INTERVAL     = 500e3;  ///< Maximum allowable time interval between GNSS measurements (uSec)
-static constexpr uint64_t GNSS_YAW_MAX_INTERVAL = 1500e3; ///< Maximum allowable time interval between GNSS yaw measurements (uSec)
-static constexpr uint64_t RNG_MAX_INTERVAL      = 200e3;  ///< Maximum allowable time interval between range finder  measurements (uSec)
+#define BARO_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between pressure altitude measurements (uSec)
+#define EV_MAX_INTERVAL   (uint64_t)2e5 ///< Maximum allowable time interval between external vision system measurements (uSec)
+#define GPS_MAX_INTERVAL  (uint64_t)5e5 ///< Maximum allowable time interval between GPS measurements (uSec)
+#define RNG_MAX_INTERVAL  (uint64_t)2e5 ///< Maximum allowable time interval between range finder  measurements (uSec)
 
 // bad accelerometer detection and mitigation
-static constexpr uint64_t BADACC_PROBATION = 10e6; ///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec)
-static constexpr float BADACC_BIAS_PNOISE = 4.9f;  ///< The delta velocity process noise is set to this when accel data is declared bad (m/sec**2)
+#define BADACC_PROBATION  (uint64_t)10e6        ///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec)
+#define BADACC_BIAS_PNOISE      4.9f    ///< The delta velocity process noise is set to this when accel data is declared bad (m/sec**2)
 
 // ground effect compensation
-static constexpr uint64_t GNDEFFECT_TIMEOUT = 10e6; ///< Maximum period of time that ground effect protection will be active after it was last turned on (uSec)
+#define GNDEFFECT_TIMEOUT       10E6    ///< Maximum period of time that ground effect protection will be active after it was last turned on (uSec)
 
 enum class VelocityFrame : uint8_t {
-	LOCAL_FRAME_NED = 0,
-	LOCAL_FRAME_FRD = 1,
-	BODY_FRAME_FRD  = 2
+	LOCAL_FRAME_FRD = 0,
+	BODY_FRAME_FRD  = 1
 };
 
 enum GeoDeclinationMask : uint8_t {
@@ -128,13 +126,6 @@ enum RngCtrl : uint8_t {
 	ENABLED     = 2
 };
 
-enum class EvCtrl : uint8_t {
-	HPOS = (1<<0),
-	VPOS = (1<<1),
-	VEL  = (1<<2),
-	YAW  = (1<<3)
-};
-
 enum SensorFusionMask : uint16_t {
 	// Bit locations for fusion_mode
 	DEPRECATED_USE_GPS = (1<<0),    ///< set to true to use GPS data (DEPRECATED, use gnss_ctrl)
@@ -145,7 +136,7 @@ enum SensorFusionMask : uint16_t {
 	USE_DRAG         = (1<<5),      ///< set to true to use the multi-rotor drag model to estimate wind
 	ROTATE_EXT_VIS   = (1<<6),      ///< set to true to if the EV observations are in a non NED reference frame and need to be rotated before being used
 	DEPRECATED_USE_GPS_YAW = (1<<7),///< set to true to use GPS yaw data if available (DEPRECATED, use gnss_ctrl)
-	DEPRECATED_USE_EXT_VIS_VEL = (1<<8), ///< set to true to use external vision velocity data
+	USE_EXT_VIS_VEL  = (1<<8),      ///< set to true to use external vision velocity data
 };
 
 struct gpsMessage {
@@ -236,11 +227,10 @@ struct extVisionSample {
 	Vector3f    vel{};         ///< FRD velocity in reference frame defined in vel_frame variable (m/sec) - Z must be aligned with down axis
 	Quatf       quat{};        ///< quaternion defining rotation from body to earth frame
 	Vector3f    posVar{};      ///< XYZ position variances (m**2)
-	Vector3f    velocity_var{};    ///< XYZ velocity variances ((m/sec)**2)
-	Vector3f    orientation_var{}; ///< orientation variance (rad**2)
+	Vector3f    velVar{};      ///< XYZ velocity variances ((m/sec)**2)
+	float       angVar{};      ///< angular heading variance (rad**2)
 	VelocityFrame vel_frame = VelocityFrame::BODY_FRAME_FRD;
 	uint8_t     reset_counter{};
-	int8_t     quality{};     ///< quality indicator between 0 and 100
 };
 
 struct dragSample {
@@ -254,14 +244,6 @@ struct auxVelSample {
 	Vector2f    velVar{};      ///< estimated error variance of the NE velocity (m/sec)**2
 };
 
-struct systemFlagUpdate {
-	uint64_t time_us{};
-	bool at_rest{false};
-	bool in_air{true};
-	bool is_fixed_wing{false};
-	bool gnd_effect{false};
-};
-
 struct stateSample {
 	Quatf    quat_nominal{};        ///< quaternion defining the rotation from body to earth frame
 	Vector3f vel{};                 ///< NED velocity in earth frame in m/s
@@ -283,7 +265,6 @@ struct parameters {
 	int32_t baro_ctrl{1};
 	int32_t gnss_ctrl{GnssCtrl::HPOS | GnssCtrl::VEL};
 	int32_t rng_ctrl{RngCtrl::CONDITIONAL};
-	int32_t ev_ctrl{0};
 	int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder |
 				    TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator
 
@@ -376,9 +357,6 @@ struct parameters {
 	float range_kin_consistency_gate{1.0f}; ///< gate size used by the range finder kinematic consistency check
 
 	// vision position fusion
-	float ev_vel_noise{0.1f};               ///< minimum allowed observation noise for EV velocity fusion (m/sec)
-	float ev_att_noise{0.1f};               ///< minimum allowed observation noise for EV attitude fusion (rad/sec)
-	int32_t ev_quality_minimum{0};          ///< vision minimum acceptable quality integer
 	float ev_vel_innov_gate{3.0f};          ///< vision velocity fusion innovation consistency gate size (STD)
 	float ev_pos_innov_gate{5.0f};          ///< vision position fusion innovation consistency gate size (STD)
 	float ev_hgt_bias_nsd{0.13f};           ///< process noise for vision height bias estimation (m/s/sqrt(Hz))
@@ -417,11 +395,11 @@ struct parameters {
 	float acc_bias_learn_gyr_lim{3.0f};     ///< learning is disabled if the magnitude of the IMU angular rate vector is greater than this (rad/sec)
 	float acc_bias_learn_tc{0.5f};          ///< time constant used to control the decaying envelope filters applied to the accel and gyro magnitudes (sec)
 
-	const unsigned reset_timeout_max{7'000'000};      ///< maximum time we allow horizontal inertial dead reckoning before attempting to reset the states to the measurement or change _control_status if the data is unavailable (uSec)
-	const unsigned no_aid_timeout_max{1'000'000};     ///< maximum lapsed time from last fusion of a measurement that constrains horizontal velocity drift before the EKF will determine that the sensor is no longer contributing to aiding (uSec)
+	const unsigned reset_timeout_max{7000000};      ///< maximum time we allow horizontal inertial dead reckoning before attempting to reset the states to the measurement or change _control_status if the data is unavailable (uSec)
+	const unsigned no_aid_timeout_max{1000000};     ///< maximum lapsed time from last fusion of a measurement that constrains horizontal velocity drift before the EKF will determine that the sensor is no longer contributing to aiding (uSec)
 	const unsigned hgt_fusion_timeout_max{5'000'000}; ///< maximum time we allow height fusion to fail before attempting a reset or stopping the fusion aiding (uSec)
 
-	int32_t valid_timeout_max{5'000'000};     ///< amount of time spent inertial dead reckoning before the estimator reports the state estimates as invalid (uSec)
+	int32_t valid_timeout_max{5000000};     ///< amount of time spent inertial dead reckoning before the estimator reports the state estimates as invalid (uSec)
 
 	// static barometer pressure position error coefficient along body axes
 	float static_pressure_coef_xp{0.0f};    // (-)
@@ -456,6 +434,7 @@ struct parameters {
 	float EKFGSF_tas_default{15.0f};                ///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s)
 	const unsigned EKFGSF_reset_delay{1000000};     ///< Number of uSec of bad innovations on main filter in immediate post-takeoff phase before yaw is reset to EKF-GSF value
 	const float EKFGSF_yaw_err_max{0.262f};         ///< Composite yaw 1-sigma uncertainty threshold used to check for convergence (rad)
+	const unsigned EKFGSF_reset_count_limit{3};     ///< Maximum number of times the yaw can be reset to the EKF-GSF yaw estimator value
 };
 
 union fault_status_u {

src/modules/ekf2/EKF/control.cpp

@@ -47,23 +47,6 @@ void Ekf::controlFusionModes()
 {
 	// Store the status to enable change detection
 	_control_status_prev.value = _control_status.value;
-	_state_reset_count_prev = _state_reset_status.reset_count;
-
-	if (_system_flag_buffer) {
-		systemFlagUpdate system_flags_delayed;
-
-		if (_system_flag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &system_flags_delayed)) {
-
-			set_vehicle_at_rest(system_flags_delayed.at_rest);
-			set_in_air_status(system_flags_delayed.in_air);
-
-			set_is_fixed_wing(system_flags_delayed.is_fixed_wing);
-
-			if (system_flags_delayed.gnd_effect) {
-				set_gnd_effect();
-			}
-		}
-	}
 
 	// monitor the tilt alignment
 	if (!_control_status.flags.tilt_align) {
@@ -103,7 +86,7 @@ void Ekf::controlFusionModes()
 	}
 
 	if (_gps_buffer) {
-		_gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL);
+		_gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GPS_MAX_INTERVAL);
 
 		// check for arrival of new sensor data at the fusion time horizon
 		_time_prev_gps_us = _gps_sample_delayed.time_us;
@@ -234,7 +217,7 @@ void Ekf::controlExternalVisionFusion()
 		// if the ev data is not in a NED reference frame, then the transformation between EV and EKF navigation frames
 		// needs to be calculated and the observations rotated into the EKF frame of reference
 		if ((_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) && ((_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_POS)
-				|| (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VEL))) && !_control_status.flags.ev_yaw) {
+				|| (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_VEL)) && !_control_status.flags.ev_yaw) {
 
 			// rotate EV measurements into the EKF Navigation frame
 			calcExtVisRotMat();
@@ -249,6 +232,11 @@ void Ekf::controlExternalVisionFusion()
 				if (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_POS && !_control_status.flags.ev_pos) {
 					startEvPosFusion();
 				}
+
+				// turn on use of external vision measurements for velocity
+				if (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_VEL && !_control_status.flags.ev_vel) {
+					startEvVelFusion();
+				}
 			}
 		}
 
@@ -343,6 +331,15 @@ void Ekf::controlExternalVisionFusion()
 
 				// check if we have been deadreckoning too long
 				if (isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)) {
+					// only reset velocity if we have no another source of aiding constraining it
+					if (isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)1E6) &&
+					    isTimedOut(_time_last_hor_vel_fuse, (uint64_t)1E6)) {
+
+						if (_control_status.flags.ev_vel) {
+							resetVelocityToVision();
+						}
+					}
+
 					resetHorizontalPositionToVision();
 				}
 			}
@@ -357,10 +354,38 @@ void Ekf::controlExternalVisionFusion()
 			fuseHorizontalPosition(_aid_src_ev_pos);
 		}
 
+		// determine if we should use the velocity observations
+		if (_control_status.flags.ev_vel) {
+			if (reset && _control_status_prev.flags.ev_vel) {
+				resetVelocityToVision();
+			}
+
+			// check if we have been deadreckoning too long
+			if (isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)) {
+				// only reset velocity if we have no another source of aiding constraining it
+				if (isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)1E6) &&
+				    isTimedOut(_time_last_hor_pos_fuse, (uint64_t)1E6)) {
+					resetVelocityToVision();
+				}
+			}
+
+			resetEstimatorAidStatus(_aid_src_ev_vel);
+
+			const Vector3f obs_var = matrix::max(getVisionVelocityVarianceInEkfFrame(), sq(0.05f));
+
+			const float innov_gate = fmaxf(_params.ev_vel_innov_gate, 1.f);
+
+			updateVelocityAidSrcStatus(_ev_sample_delayed.time_us, getVisionVelocityInEkfFrame(), obs_var, innov_gate, _aid_src_ev_vel);
+
+			_aid_src_ev_vel.fusion_enabled = true;
+
+			fuseVelocity(_aid_src_ev_vel);
+		}
+
 		// determine if we should use the yaw observation
 		resetEstimatorAidStatus(_aid_src_ev_yaw);
 		const float measured_hdg = getEulerYaw(_ev_sample_delayed.quat);
-		const float ev_yaw_obs_var = fmaxf(_ev_sample_delayed.orientation_var(2), 1.e-4f);
+		const float ev_yaw_obs_var = fmaxf(_ev_sample_delayed.angVar, 1.e-4f);
 
 		if (PX4_ISFINITE(measured_hdg)) {
 			_aid_src_ev_yaw.timestamp_sample = _ev_sample_delayed.time_us;
@@ -389,28 +414,12 @@ void Ekf::controlExternalVisionFusion()
 			}
 		}
 
-
-		bool ev_reset = (_ev_sample_delayed.reset_counter != _ev_sample_delayed_prev.reset_counter);
-
-		// determine if we should use the horizontal position observations
-		bool quality_sufficient = (_params.ev_quality_minimum <= 0) || (_ev_sample_delayed.quality >= _params.ev_quality_minimum);
-
-		bool starting_conditions_passing = quality_sufficient
-						   && ((_ev_sample_delayed.time_us - _ev_sample_delayed_prev.time_us) < EV_MAX_INTERVAL)
-						   && ((_params.ev_quality_minimum <= 0) || (_ev_sample_delayed_prev.quality >= _params.ev_quality_minimum)) // previous quality sufficient
-						   && ((_params.ev_quality_minimum <= 0) || (_ext_vision_buffer->get_newest().quality >= _params.ev_quality_minimum)) // newest quality sufficient
-						   && isNewestSampleRecent(_time_last_ext_vision_buffer_push, EV_MAX_INTERVAL);
-
-		// EV velocity
-		controlEvVelFusion(_ev_sample_delayed, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel);
-
-
 		// record observation and estimate for use next time
 		_ev_sample_delayed_prev = _ev_sample_delayed;
 		_hpos_pred_prev = _state.pos.xy();
 		_yaw_pred_prev = getEulerYaw(_state.quat_nominal);
 
-	} else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.ev_yaw)
+	} else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel ||  _control_status.flags.ev_yaw)
 		   && !isRecent(_ev_sample_delayed.time_us, (uint64_t)_params.reset_timeout_max)) {
 
 		// Turn off EV fusion mode if no data has been received
@@ -435,7 +444,7 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi
 
 		const bool continuing_conditions_passing = !gps_checks_failing;
 
-		const bool is_gps_yaw_data_intermittent = !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, 2 * GNSS_YAW_MAX_INTERVAL);
+		const bool is_gps_yaw_data_intermittent = !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, 2 * GPS_MAX_INTERVAL);
 
 		const bool starting_conditions_passing = continuing_conditions_passing
 				&& _control_status.flags.tilt_align
@@ -629,3 +638,10 @@ void Ekf::controlAuxVelFusion()
 		}
 	}
 }
+
+bool Ekf::hasHorizontalAidingTimedOut() const
+{
+	return isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
+	       && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
+	       && isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max);
+}

src/modules/ekf2/EKF/ekf.cpp

@@ -204,6 +204,8 @@ bool Ekf::initialiseFilter()
 	_time_last_hgt_fuse = _imu_sample_delayed.time_us;
 	_time_last_hor_pos_fuse = _imu_sample_delayed.time_us;
 	_time_last_hor_vel_fuse = _imu_sample_delayed.time_us;
+	_time_last_hagl_fuse = _imu_sample_delayed.time_us;
+	_time_last_flow_terrain_fuse = _imu_sample_delayed.time_us;
 
 	// reset the output predictor state history to match the EKF initial values
 	alignOutputFilter();

src/modules/ekf2/EKF/ekf.h

@@ -285,7 +285,7 @@ public:
 		return !_vertical_velocity_deadreckon_time_exceeded && !_control_status.flags.fake_hgt;
 	}
 
-	bool isTerrainEstimateValid() const;
+	bool isTerrainEstimateValid() const { return _hagl_valid; };
 
 	bool isYawFinalAlignComplete() const
 	{
@@ -335,43 +335,43 @@ public:
 	const auto &state_reset_status() const { return _state_reset_status; }
 
 	// return the amount the local vertical position changed in the last reset and the number of reset events
-	uint8_t get_posD_reset_count() const { return _state_reset_status.reset_count.posD; }
+	uint8_t get_posD_reset_count() const { return _state_reset_status.posD_counter; }
 	void get_posD_reset(float *delta, uint8_t *counter) const
 	{
 		*delta = _state_reset_status.posD_change;
-		*counter = _state_reset_status.reset_count.posD;
+		*counter = _state_reset_status.posD_counter;
 	}
 
 	// return the amount the local vertical velocity changed in the last reset and the number of reset events
-	uint8_t get_velD_reset_count() const { return _state_reset_status.reset_count.velD; }
+	uint8_t get_velD_reset_count() const { return _state_reset_status.velD_counter; }
 	void get_velD_reset(float *delta, uint8_t *counter) const
 	{
 		*delta = _state_reset_status.velD_change;
-		*counter = _state_reset_status.reset_count.velD;
+		*counter = _state_reset_status.velD_counter;
 	}
 
 	// return the amount the local horizontal position changed in the last reset and the number of reset events
-	uint8_t get_posNE_reset_count() const { return _state_reset_status.reset_count.posNE; }
+	uint8_t get_posNE_reset_count() const { return _state_reset_status.posNE_counter; }
 	void get_posNE_reset(float delta[2], uint8_t *counter) const
 	{
 		_state_reset_status.posNE_change.copyTo(delta);
-		*counter = _state_reset_status.reset_count.posNE;
+		*counter = _state_reset_status.posNE_counter;
 	}
 
 	// return the amount the local horizontal velocity changed in the last reset and the number of reset events
-	uint8_t get_velNE_reset_count() const { return _state_reset_status.reset_count.velNE; }
+	uint8_t get_velNE_reset_count() const { return _state_reset_status.velNE_counter; }
 	void get_velNE_reset(float delta[2], uint8_t *counter) const
 	{
 		_state_reset_status.velNE_change.copyTo(delta);
-		*counter = _state_reset_status.reset_count.velNE;
+		*counter = _state_reset_status.velNE_counter;
 	}
 
 	// return the amount the quaternion has changed in the last reset and the number of reset events
-	uint8_t get_quat_reset_count() const { return _state_reset_status.reset_count.quat; }
+	uint8_t get_quat_reset_count() const { return _state_reset_status.quat_counter; }
 	void get_quat_reset(float delta_quat[4], uint8_t *counter) const
 	{
 		_state_reset_status.quat_change.copyTo(delta_quat);
-		*counter = _state_reset_status.reset_count.quat;
+		*counter = _state_reset_status.quat_counter;
 	}
 
 	// get EKF innovation consistency check status information comprising of:
@@ -451,27 +451,20 @@ private:
 	void updateHorizontalDeadReckoningstatus();
 	void updateVerticalDeadReckoningStatus();
 
-	struct StateResetCounts
-	{
-		uint8_t velNE{0};	///< number of horizontal position reset events (allow to wrap if count exceeds 255)
-		uint8_t velD{0};	///< number of vertical velocity reset events (allow to wrap if count exceeds 255)
-		uint8_t posNE{0};	///< number of horizontal position reset events (allow to wrap if count exceeds 255)
-		uint8_t posD{0};	///< number of vertical position reset events (allow to wrap if count exceeds 255)
-		uint8_t quat{0};	///< number of quaternion reset events (allow to wrap if count exceeds 255)
-	};
+	void updateTerrainValidity();
 
-	struct StateResets {
+	struct {
+		uint8_t velNE_counter;	///< number of horizontal position reset events (allow to wrap if count exceeds 255)
+		uint8_t velD_counter;	///< number of vertical velocity reset events (allow to wrap if count exceeds 255)
+		uint8_t posNE_counter;	///< number of horizontal position reset events (allow to wrap if count exceeds 255)
+		uint8_t posD_counter;	///< number of vertical position reset events (allow to wrap if count exceeds 255)
+		uint8_t quat_counter;	///< number of quaternion reset events (allow to wrap if count exceeds 255)
 		Vector2f velNE_change;  ///< North East velocity change due to last reset (m)
 		float velD_change;	///< Down velocity change due to last reset (m/sec)
 		Vector2f posNE_change;	///< North, East position change due to last reset (m)
 		float posD_change;	///< Down position change due to last reset (m)
 		Quatf quat_change;	///< quaternion delta due to last reset - multiply pre-reset quaternion by this to get post-reset quaternion
-
-		StateResetCounts reset_count{};
-	};
-
-	StateResets _state_reset_status{};	///< reset event monitoring structure containing velocity, position, height and yaw reset information
-	StateResetCounts _state_reset_count_prev{};
+	} _state_reset_status{};	///< reset event monitoring structure containing velocity, position, height and yaw reset information
 
 	Vector3f _ang_rate_delayed_raw{};	///< uncorrected angular rate vector at fusion time horizon (rad/sec)
 
@@ -511,8 +504,6 @@ private:
 	uint64_t _time_last_healthy_rng_data{0};
 	uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source
 
-	uint8_t _nb_ev_vel_reset_available{0};
-
 	Vector3f _last_known_pos{};		///< last known local position vector (m)
 
 	uint64_t _time_acc_bias_check{0};	///< last time the  accel bias check passed (uSec)
@@ -637,6 +628,7 @@ private:
 	float _terrain_var{1e4f};		///< variance of terrain position estimate (m**2)
 	uint8_t _terrain_vpos_reset_counter{0};	///< number of times _terrain_vpos has been reset
 	uint64_t _time_last_hagl_fuse{0};		///< last system time that a range sample was fused by the terrain estimator
+	bool _hagl_valid{false};		///< true when the height above ground estimate is valid
 	terrain_fusion_status_u _hagl_sensor_status{}; ///< Struct indicating type of sensor used to estimate height above ground
 
 	// height sensor status
@@ -708,6 +700,7 @@ private:
 	void resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f &new_horz_vel_var);
 	void resetHorizontalVelocityTo(const Vector2f &new_horz_vel, float vel_var) { resetHorizontalVelocityTo(new_horz_vel, Vector2f(vel_var, vel_var)); }
 
+	void resetVelocityToVision();
 	void resetHorizontalVelocityToZero();
 
 	void resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var);
@@ -726,8 +719,6 @@ private:
 	// fuse optical flow line of sight rate measurements
 	void updateOptFlow(estimator_aid_source2d_s &aid_src);
 	void fuseOptFlow();
-	float predictFlowRange();
-	Vector2f predictFlowVelBody();
 
 	// horizontal and vertical position aid source
 	void updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source2d_s &aid_src) const;
@@ -774,6 +765,7 @@ private:
 	void fuseFlowForTerrain();
 
 	void controlHaglFakeFusion();
+	void resetHaglFake();
 
 	// reset the heading and magnetic field states using the declination and magnetometer measurements
 	// return true if successful
@@ -792,6 +784,10 @@ private:
 	// update the rotation matrix which transforms EV navigation frame measurements into NED
 	void calcExtVisRotMat();
 
+	Vector3f getVisionVelocityInEkfFrame() const;
+
+	Vector3f getVisionVelocityVarianceInEkfFrame() const;
+
 	// matrix vector multiplication for computing K<24,1> * H<1,24> * P<24,24>
 	// that is optimized by exploring the sparsity in H
 	template <size_t ...Idxs>
@@ -876,7 +872,6 @@ private:
 
 	// control fusion of external vision observations
 	void controlExternalVisionFusion();
-	void controlEvVelFusion(const extVisionSample &ev_sample, bool starting_conditions_passing, bool ev_reset, bool quality_sufficient, estimator_aid_source3d_s& aid_src);
 
 	// control fusion of optical flow observations
 	void controlOpticalFlowFusion();
@@ -886,6 +881,7 @@ private:
 	// control fusion of GPS observations
 	void controlGpsFusion();
 	bool shouldResetGpsFusion() const;
+	bool hasHorizontalAidingTimedOut() const;
 	bool isYawFailure() const;
 
 	void controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing);
@@ -1037,6 +1033,7 @@ private:
 	void stopGpsYawFusion();
 
 	void startEvPosFusion();
+	void startEvVelFusion();
 	void startEvYawFusion();
 
 	void stopEvFusion();
@@ -1070,6 +1067,9 @@ private:
 	HeightBiasEstimator _rng_hgt_b_est{HeightSensor::RANGE, _height_sensor_ref};
 	HeightBiasEstimator _ev_hgt_b_est{HeightSensor::EV, _height_sensor_ref};
 
+	int64_t _ekfgsf_yaw_reset_time{0};	///< timestamp of last emergency yaw reset (uSec)
+	uint8_t _ekfgsf_yaw_reset_count{0};	// number of times the yaw has been reset to the EKF-GSF estimate
+
 	void runYawEKFGSF();
 
 	// Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -44,6 +44,13 @@
 #include <mathlib/mathlib.h>
 #include <cstdlib>
 
+void Ekf::resetVelocityToVision()
+{
+	_information_events.flags.reset_vel_to_vision = true;
+	ECL_INFO("reset to vision velocity");
+	resetVelocityTo(getVisionVelocityInEkfFrame(), getVisionVelocityVarianceInEkfFrame());
+}
+
 void Ekf::resetHorizontalVelocityToZero()
 {
 	_information_events.flags.reset_vel_to_zero = true;
@@ -77,16 +84,8 @@ void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f
 
 	_output_new.vel.xy() += delta_horz_vel;
 
-	// record the state change
-	if (_state_reset_status.reset_count.velNE == _state_reset_count_prev.velNE) {
-		_state_reset_status.velNE_change = delta_horz_vel;
-
-	} else {
-		// there's already a reset this update, accumulate total delta
-		_state_reset_status.velNE_change += delta_horz_vel;
-	}
-
-	_state_reset_status.reset_count.velNE++;
+	_state_reset_status.velNE_change = delta_horz_vel;
+	_state_reset_status.velNE_counter++;
 
 	// Reset the timout timer
 	_time_last_hor_vel_fuse = _imu_sample_delayed.time_us;
@@ -109,16 +108,8 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var)
 	_output_new.vel(2) += delta_vert_vel;
 	_output_vert_new.vert_vel += delta_vert_vel;
 
-	// record the state change
-	if (_state_reset_status.reset_count.velD == _state_reset_count_prev.velD) {
-		_state_reset_status.velD_change = delta_vert_vel;
-
-	} else {
-		// there's already a reset this update, accumulate total delta
-		_state_reset_status.velD_change += delta_vert_vel;
-	}
-
-	_state_reset_status.reset_count.velD++;
+	_state_reset_status.velD_change = delta_vert_vel;
+	_state_reset_status.velD_counter++;
 
 	// Reset the timout timer
 	_time_last_ver_vel_fuse = _imu_sample_delayed.time_us;
@@ -167,16 +158,8 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f
 
 	_output_new.pos.xy() += delta_horz_pos;
 
-	// record the state change
-	if (_state_reset_status.reset_count.posNE == _state_reset_count_prev.posNE) {
-		_state_reset_status.posNE_change = delta_horz_pos;
-
-	} else {
-		// there's already a reset this update, accumulate total delta
-		_state_reset_status.posNE_change += delta_horz_pos;
-	}
-
-	_state_reset_status.reset_count.posNE++;
+	_state_reset_status.posNE_change = delta_horz_pos;
+	_state_reset_status.posNE_counter++;
 
 	// Reset the timout timer
 	_time_last_hor_pos_fuse = _imu_sample_delayed.time_us;
@@ -204,36 +187,27 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v
 		P.uncorrelateCovarianceSetVariance<1>(9, math::max(sq(0.01f), new_vert_pos_var));
 	}
 
-	const float delta_z = new_vert_pos - old_vert_pos;
+	// store the reset amount and time to be published
+	_state_reset_status.posD_change = new_vert_pos - old_vert_pos;
+	_state_reset_status.posD_counter++;
 
 	// apply the change in height / height rate to our newest height / height rate estimate
 	// which have already been taken out from the output buffer
-	_output_new.pos(2) += delta_z;
+	_output_new.pos(2) += _state_reset_status.posD_change;
 
 	// add the reset amount to the output observer buffered data
 	for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
-		_output_buffer[i].pos(2) += delta_z;
-		_output_vert_buffer[i].vert_vel_integ += delta_z;
+		_output_buffer[i].pos(2) += _state_reset_status.posD_change;
+		_output_vert_buffer[i].vert_vel_integ += _state_reset_status.posD_change;
 	}
 
 	// add the reset amount to the output observer vertical position state
 	_output_vert_new.vert_vel_integ = _state.pos(2);
 
-	// record the state change
-	if (_state_reset_status.reset_count.posD == _state_reset_count_prev.posD) {
-		_state_reset_status.posD_change = delta_z;
-
-	} else {
-		// there's already a reset this update, accumulate total delta
-		_state_reset_status.posD_change += delta_z;
-	}
-
-	_state_reset_status.reset_count.posD++;
-
-	_baro_b_est.setBias(_baro_b_est.getBias() + delta_z);
-	_ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - delta_z);
-	_gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + delta_z);
-	_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z);
+	_baro_b_est.setBias(_baro_b_est.getBias() + _state_reset_status.posD_change);
+	_ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - _state_reset_status.posD_change);
+	_gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + _state_reset_status.posD_change);
+	_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + _state_reset_status.posD_change);
 
 	// Reset the timout timer
 	_time_last_hgt_fuse = _imu_sample_delayed.time_us;
@@ -323,7 +297,7 @@ bool Ekf::resetMagHeading()
 bool Ekf::resetYawToEv()
 {
 	const float yaw_new = getEulerYaw(_ev_sample_delayed.quat);
-	const float yaw_new_variance = fmaxf(_ev_sample_delayed.orientation_var(2), sq(1.0e-2f));
+	const float yaw_new_variance = fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f));
 
 	resetQuatStateYaw(yaw_new, yaw_new_variance);
 	_R_ev_to_ekf.setIdentity();
@@ -889,8 +863,6 @@ void Ekf::fuse(const Vector24f &K, float innovation)
 {
 	_state.quat_nominal -= K.slice<4, 1>(0, 0) * innovation;
 	_state.quat_nominal.normalize();
-	_R_to_earth = Dcmf(_state.quat_nominal);
-
 	_state.vel -= K.slice<3, 1>(4, 0) * innovation;
 	_state.pos -= K.slice<3, 1>(7, 0) * innovation;
 	_state.delta_ang_bias -= K.slice<3, 1>(10, 0) * innovation;
@@ -1184,6 +1156,56 @@ void Ekf::updateGroundEffect()
 	}
 }
 
+Vector3f Ekf::getVisionVelocityInEkfFrame() const
+{
+	Vector3f vel;
+	// correct velocity for offset relative to IMU
+	const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
+	const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
+
+	// rotate measurement into correct earth frame if required
+	switch (_ev_sample_delayed.vel_frame) {
+	case VelocityFrame::BODY_FRAME_FRD:
+		vel = _R_to_earth * (_ev_sample_delayed.vel - vel_offset_body);
+		break;
+
+	case VelocityFrame::LOCAL_FRAME_FRD:
+		const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
+
+		if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) {
+			vel = _R_ev_to_ekf * _ev_sample_delayed.vel - vel_offset_earth;
+
+		} else {
+			vel = _ev_sample_delayed.vel - vel_offset_earth;
+		}
+
+		break;
+	}
+
+	return vel;
+}
+
+Vector3f Ekf::getVisionVelocityVarianceInEkfFrame() const
+{
+	Matrix3f ev_vel_cov = matrix::diag(_ev_sample_delayed.velVar);
+
+	// rotate measurement into correct earth frame if required
+	switch (_ev_sample_delayed.vel_frame) {
+	case VelocityFrame::BODY_FRAME_FRD:
+		ev_vel_cov = _R_to_earth * ev_vel_cov * _R_to_earth.transpose();
+		break;
+
+	case VelocityFrame::LOCAL_FRAME_FRD:
+		if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) {
+			ev_vel_cov = _R_ev_to_ekf * ev_vel_cov * _R_ev_to_ekf.transpose();
+		}
+
+		break;
+	}
+
+	return ev_vel_cov.diag();
+}
+
 // update the rotation matrix which rotates EV measurements into the EKF's navigation frame
 void Ekf::calcExtVisRotMat()
 {
@@ -1346,6 +1368,14 @@ void Ekf::startEvPosFusion()
 	ECL_INFO("starting vision pos fusion");
 }
 
+void Ekf::startEvVelFusion()
+{
+	_control_status.flags.ev_vel = true;
+	resetVelocityToVision();
+	_information_events.flags.starting_vision_vel_fusion = true;
+	ECL_INFO("starting vision vel fusion");
+}
+
 void Ekf::startEvYawFusion()
 {
 	// turn on fusion of external vision yaw measurements and disable all magnetometer fusion
@@ -1375,6 +1405,15 @@ void Ekf::stopEvPosFusion()
 	}
 }
 
+void Ekf::stopEvVelFusion()
+{
+	if (_control_status.flags.ev_vel) {
+		ECL_INFO("stopping EV vel fusion");
+		_control_status.flags.ev_vel = false;
+		resetEstimatorAidStatus(_aid_src_ev_vel);
+	}
+}
+
 void Ekf::stopEvYawFusion()
 {
 	if (_control_status.flags.ev_yaw) {
@@ -1417,6 +1456,9 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
 	_state.quat_nominal = quat_after_reset;
 	uncorrelateQuatFromOtherStates();
 
+	// record the state change
+	_state_reset_status.quat_change = q_error;
+
 	// update the yaw angle variance
 	if (yaw_variance > FLT_EPSILON) {
 		increaseQuatYawErrVariance(yaw_variance);
@@ -1424,26 +1466,17 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
 
 	// add the reset amount to the output observer buffered data
 	for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
-		_output_buffer[i].quat_nominal = q_error * _output_buffer[i].quat_nominal;
+		_output_buffer[i].quat_nominal = _state_reset_status.quat_change * _output_buffer[i].quat_nominal;
 	}
 
 	// apply the change in attitude quaternion to our newest quaternion estimate
 	// which was already taken out from the output buffer
-	_output_new.quat_nominal = q_error * _output_new.quat_nominal;
-
-	// record the state change
-	if (_state_reset_status.reset_count.quat == _state_reset_count_prev.quat) {
-		_state_reset_status.quat_change = q_error;
-
-	} else {
-		// there's already a reset this update, accumulate total delta
-		_state_reset_status.quat_change = q_error * _state_reset_status.quat_change;
-		_state_reset_status.quat_change.normalize();
-	}
-
-	_state_reset_status.reset_count.quat++;
+	_output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal;
 
 	_last_static_yaw = NAN;
+
+	// capture the reset event
+	_state_reset_status.quat_counter++;
 }
 
 // Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator
@@ -1475,6 +1508,9 @@ bool Ekf::resetYawToEKFGSF()
 		_inhibit_ev_yaw_use = true;
 	}
 
+	_ekfgsf_yaw_reset_time = _imu_sample_delayed.time_us;
+	_ekfgsf_yaw_reset_count++;
+
 	return true;
 }
 

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -410,42 +410,6 @@ void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample)
 	}
 }
 
-void EstimatorInterface::setSystemFlagData(const systemFlagUpdate &system_flags)
-{
-	if (!_initialised) {
-		return;
-	}
-
-	// Allocate the required buffer size if not previously done
-	if (_system_flag_buffer == nullptr) {
-		_system_flag_buffer = new RingBuffer<systemFlagUpdate>(_obs_buffer_length);
-
-		if (_system_flag_buffer == nullptr || !_system_flag_buffer->valid()) {
-			delete _system_flag_buffer;
-			_system_flag_buffer = nullptr;
-			printBufferAllocationFailed("system flag");
-			return;
-		}
-	}
-
-	_system_flag_buffer->push(system_flags);
-
-	const int64_t time_us = system_flags.time_us
-				- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
-
-	// limit data rate to prevent data being lost
-	if (time_us >= static_cast<int64_t>(_system_flag_buffer->get_newest().time_us + _min_obs_interval_us)) {
-
-		systemFlagUpdate system_flags_new{system_flags};
-		system_flags_new.time_us = time_us;
-
-		_system_flag_buffer->push(system_flags_new);
-
-	} else {
-		ECL_WARN("system flag update too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _system_flag_buffer->get_newest().time_us, _min_obs_interval_us);
-	}
-}
-
 void EstimatorInterface::setDragData(const imuSample &imu)
 {
 	// down-sample the drag specific force data by accumulating and calculating the mean when
@@ -532,14 +496,14 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 		max_time_delay_ms = math::max(_params.flow_delay_ms, max_time_delay_ms);
 	}
 
-	if ((_params.fusion_mode & (SensorFusionMask::USE_EXT_VIS_POS | SensorFusionMask::USE_EXT_VIS_YAW)) || (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VEL))) {
+	if (_params.fusion_mode & (SensorFusionMask::USE_EXT_VIS_POS | SensorFusionMask::USE_EXT_VIS_YAW | SensorFusionMask::USE_EXT_VIS_VEL)) {
 		max_time_delay_ms = math::max(_params.ev_delay_ms, max_time_delay_ms);
 	}
 
 	const float filter_update_period_ms = _params.filter_update_interval_us / 1000.f;
 
 	// calculate the IMU buffer length required to accomodate the maximum delay with some allowance for jitter
-	_imu_buffer_length = math::max(2, (int)ceilf(max_time_delay_ms / filter_update_period_ms));
+	_imu_buffer_length = ceilf(max_time_delay_ms / filter_update_period_ms);
 
 	// set the observation buffer length to handle the minimum time of arrival between observations in combination
 	// with the worst case delay from current time to ekf fusion time

src/modules/ekf2/EKF/estimator_interface.h

@@ -101,8 +101,6 @@ public:
 
 	void setAuxVelData(const auxVelSample &auxvel_sample);
 
-	void setSystemFlagData(const systemFlagUpdate &system_flags);
-
 	// return a address to the parameters struct
 	// in order to give access to the application
 	parameters *getParamHandle() { return &_params; }
@@ -357,10 +355,9 @@ protected:
 	uint64_t _time_last_in_air{0};		///< last time we were in air (uSec)
 
 	// data buffer instances
-	static constexpr uint8_t kBufferLengthDefault = 12;
-	RingBuffer<imuSample> _imu_buffer{kBufferLengthDefault};
-	RingBuffer<outputSample> _output_buffer{kBufferLengthDefault};
-	RingBuffer<outputVert> _output_vert_buffer{kBufferLengthDefault};
+	RingBuffer<imuSample> _imu_buffer{12};           // buffer length 12 with default parameters
+	RingBuffer<outputSample> _output_buffer{12};
+	RingBuffer<outputVert> _output_vert_buffer{12};
 
 	RingBuffer<gpsSample> *_gps_buffer{nullptr};
 	RingBuffer<magSample> *_mag_buffer{nullptr};
@@ -371,7 +368,6 @@ protected:
 	RingBuffer<extVisionSample> *_ext_vision_buffer{nullptr};
 	RingBuffer<dragSample> *_drag_buffer{nullptr};
 	RingBuffer<auxVelSample> *_auxvel_buffer{nullptr};
-	RingBuffer<systemFlagUpdate> *_system_flag_buffer{nullptr};
 
 	uint64_t _time_last_gps_buffer_push{0};
 	uint64_t _time_last_gps_yaw_buffer_push{0};

src/modules/ekf2/EKF/ev_height_control.cpp

@@ -94,28 +94,8 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample)
 					bias_est.setBias(-_state.pos(2) + measurement);
 
 					// reset vertical velocity
-					if (ev_sample.vel.isAllFinite() && (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VEL))) {
-
-						// correct velocity for offset relative to IMU
-						const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
-						const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
-						const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
-
-						switch (ev_sample.vel_frame) {
-						case VelocityFrame::LOCAL_FRAME_NED:
-						case VelocityFrame::LOCAL_FRAME_FRD: {
-								const Vector3f reset_vel = ev_sample.vel - vel_offset_earth;
-								resetVerticalVelocityTo(reset_vel(2), math::max(ev_sample.velocity_var(2), sq(_params.ev_vel_noise)));
-							}
-							break;
-
-						case VelocityFrame::BODY_FRAME_FRD: {
-								const Vector3f reset_vel = _R_to_earth * (ev_sample.vel - vel_offset_body);
-								const Matrix3f reset_vel_cov = _R_to_earth * matrix::diag(ev_sample.velocity_var) * _R_to_earth.transpose();
-								resetVerticalVelocityTo(reset_vel(2), math::max(reset_vel_cov(2, 2), sq(_params.ev_vel_noise)));
-							}
-							break;
-						}
+					if (PX4_ISFINITE(ev_sample.vel(2)) && (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_POS)) {
+						resetVerticalVelocityTo(ev_sample.vel(2), ev_sample.velVar(2));
 
 					} else {
 						resetVerticalVelocityToZero();

src/modules/ekf2/EKF/ev_vel_control.cpp

@@ -1,231 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2022 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 ev_vel_control.cpp
- * Control functions for ekf external vision velocity fusion
- */
-
-#include "ekf.h"
-
-void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, bool starting_conditions_passing, bool ev_reset,
-			     bool quality_sufficient, estimator_aid_source3d_s &aid_src)
-{
-	static constexpr const char *AID_SRC_NAME = "EV velocity";
-
-	const bool yaw_alignment_changed = (!_control_status_prev.flags.ev_yaw && _control_status.flags.ev_yaw)
-					   || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);
-
-	// determine if we should use EV velocity aiding
-	bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VEL))
-					     && _control_status.flags.tilt_align
-					     && ev_sample.vel.isAllFinite();
-
-	// correct velocity for offset relative to IMU
-	const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
-	const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
-	const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
-
-	// rotate measurement into correct earth frame if required
-	Vector3f vel{NAN, NAN, NAN};
-	Matrix3f vel_cov{};
-
-	switch (ev_sample.vel_frame) {
-	case VelocityFrame::LOCAL_FRAME_NED:
-		if (_control_status.flags.yaw_align) {
-			vel = ev_sample.vel - vel_offset_earth;
-			vel_cov = matrix::diag(ev_sample.velocity_var);
-
-		} else {
-			continuing_conditions_passing = false;
-		}
-
-		break;
-
-	case VelocityFrame::LOCAL_FRAME_FRD:
-		if (_control_status.flags.ev_yaw) {
-			// using EV frame
-			vel = ev_sample.vel - vel_offset_earth;
-			vel_cov = matrix::diag(ev_sample.velocity_var);
-
-		} else {
-			// rotate EV to the EKF reference frame
-			const Quatf q_error((_state.quat_nominal * ev_sample.quat.inversed()).normalized());
-			const Dcmf R_ev_to_ekf = Dcmf(q_error);
-
-			vel = R_ev_to_ekf * ev_sample.vel - vel_offset_earth;
-			vel_cov = R_ev_to_ekf * matrix::diag(ev_sample.velocity_var) * R_ev_to_ekf.transpose();
-
-			// increase minimum variance to include EV orientation variance
-			// TODO: do this properly
-			const float orientation_var_max = ev_sample.orientation_var.max();
-
-			for (int i = 0; i < 2; i++) {
-				vel_cov(i, i) = math::max(vel_cov(i, i), orientation_var_max);
-			}
-		}
-
-		break;
-
-	case VelocityFrame::BODY_FRAME_FRD:
-		vel = _R_to_earth * (ev_sample.vel - vel_offset_body);
-		vel_cov = _R_to_earth * matrix::diag(ev_sample.velocity_var) * _R_to_earth.transpose();
-		break;
-
-	default:
-		continuing_conditions_passing = false;
-		break;
-	}
-
-	// increase minimum variance if GPS active (position reference)
-	if (_control_status.flags.gps) {
-		for (int i = 0; i < 2; i++) {
-			vel_cov(i, i) = math::max(vel_cov(i, i), sq(_params.gps_vel_noise));
-		}
-	}
-
-	const Vector3f measurement{vel};
-
-	const Vector3f measurement_var{
-		math::max(vel_cov(0, 0), sq(_params.ev_vel_noise), sq(0.01f)),
-		math::max(vel_cov(1, 1), sq(_params.ev_vel_noise), sq(0.01f)),
-		math::max(vel_cov(2, 2), sq(_params.ev_vel_noise), sq(0.01f))
-	};
-
-	const bool measurement_valid = measurement.isAllFinite() && measurement_var.isAllFinite();
-
-	updateVelocityAidSrcStatus(ev_sample.time_us,
-				   measurement,                               // observation
-				   measurement_var,                           // observation variance
-				   math::max(_params.ev_vel_innov_gate, 1.f), // innovation gate
-				   aid_src);
-
-	if (!measurement_valid) {
-		continuing_conditions_passing = false;
-	}
-
-	starting_conditions_passing &= continuing_conditions_passing
-				       && ((Vector3f(aid_src.test_ratio).max() < 0.1f) || !isHorizontalAidingActive());
-
-	if (_control_status.flags.ev_vel) {
-		aid_src.fusion_enabled = true;
-
-		if (continuing_conditions_passing) {
-
-			if ((ev_reset && isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.ev_vel)) || yaw_alignment_changed) {
-
-				if (quality_sufficient) {
-					ECL_INFO("reset to %s", AID_SRC_NAME);
-					_information_events.flags.reset_vel_to_vision = true;
-					resetVelocityTo(measurement, measurement_var);
-					aid_src.time_last_fuse = _imu_sample_delayed.time_us;
-
-				} else {
-					// EV has reset, but quality isn't sufficient
-					// we have no choice but to stop EV and try to resume once quality is acceptable
-					stopEvVelFusion();
-					return;
-				}
-
-			} else if (quality_sufficient) {
-				fuseVelocity(aid_src);
-
-			} else {
-				aid_src.innovation_rejected = true;
-			}
-
-			const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.no_aid_timeout_max); // 1 second
-
-			if (is_fusion_failing) {
-
-				if ((_nb_ev_vel_reset_available > 0) && quality_sufficient) {
-					// Data seems good, attempt a reset
-					_information_events.flags.reset_vel_to_vision = true;
-					ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME);
-					resetVelocityTo(measurement, measurement_var);
-					aid_src.time_last_fuse = _imu_sample_delayed.time_us;
-
-					if (_control_status.flags.in_air) {
-						_nb_ev_vel_reset_available--;
-					}
-
-				} else if (starting_conditions_passing) {
-					// Data seems good, but previous reset did not fix the issue
-					// something else must be wrong, declare the sensor faulty and stop the fusion
-					//_control_status.flags.ev_vel_fault = true;
-					ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME);
-					stopEvVelFusion();
-
-				} else {
-					// A reset did not fix the issue but all the starting checks are not passing
-					// This could be a temporary issue, stop the fusion without declaring the sensor faulty
-					ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME);
-					stopEvVelFusion();
-				}
-			}
-
-		} else {
-			// Stop fusion but do not declare it faulty
-			ECL_WARN("stopping %s fusion, continuing conditions failing", AID_SRC_NAME);
-			stopEvVelFusion();
-		}
-
-	} else {
-		if (starting_conditions_passing) {
-			// activate fusion, only reset if necessary
-			if (!isHorizontalAidingActive() || yaw_alignment_changed) {
-				ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME);
-				_information_events.flags.reset_vel_to_vision = true;
-				resetVelocityTo(measurement, measurement_var);
-
-			} else {
-				ECL_INFO("starting %s fusion", AID_SRC_NAME);
-			}
-
-			aid_src.time_last_fuse = _imu_sample_delayed.time_us;
-
-			_nb_ev_vel_reset_available = 5;
-			_information_events.flags.starting_vision_vel_fusion = true;
-			_control_status.flags.ev_vel = true;
-		}
-	}
-}
-
-void Ekf::stopEvVelFusion()
-{
-	if (_control_status.flags.ev_vel) {
-		resetEstimatorAidStatus(_aid_src_ev_vel);
-
-		_control_status.flags.ev_vel = false;
-	}
-}

src/modules/ekf2/EKF/gnss_height_control.cpp

@@ -92,7 +92,7 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample)
 				&& _gps_checks_passed;
 
 		const bool starting_conditions_passing = continuing_conditions_passing
-				&& isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL)
+				&& isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GPS_MAX_INTERVAL)
 				&& _gps_checks_passed
 				&& gps_checks_passing
 				&& !gps_checks_failing;
@@ -158,7 +158,7 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample)
 		}
 
 	} else if (_control_status.flags.gps_hgt
-		   && !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL)) {
+		   && !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GPS_MAX_INTERVAL)) {
 		// No data anymore. Stop until it comes back.
 		ECL_WARN("stopping %s height fusion, no data", HGT_SRC_NAME);
 		stopGpsHgtFusion();

src/modules/ekf2/EKF/gps_control.cpp

@@ -112,24 +112,25 @@ void Ekf::controlGpsFusion()
 					fuseVelocity(_aid_src_gnss_vel);
 					fuseHorizontalPosition(_aid_src_gnss_pos);
 
-					bool do_vel_pos_reset = shouldResetGpsFusion();
+					if (shouldResetGpsFusion()) {
+						const bool was_gps_signal_lost = isTimedOut(_time_prev_gps_us, 1'000'000);
 
-					if (isYawFailure()
-					    && _control_status.flags.in_air
-					    && isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay)
-					    && (_time_last_hor_vel_fuse > _time_last_on_ground_us)) {
-						/* A rapid reset to the yaw emergency estimate is performed if horizontal velocity innovation checks continuously
-						 * fails while the difference between the yaw emergency estimator and the yas estimate is large.
-						 * This enables recovery from a bad yaw estimate. A reset is not performed if the fault condition was
-						 * present before flight to prevent triggering due to GPS glitches or other sensor errors.
+						/* A reset is not performed when getting GPS back after a significant period of no data
+						 * because the timeout could have been caused by bad GPS.
+						 * The total number of resets allowed per boot cycle is limited.
 						 */
-						if (resetYawToEKFGSF()) {
-							ECL_WARN("GPS emergency yaw reset");
-							do_vel_pos_reset = true;
+						if (isYawFailure()
+						    && _control_status.flags.in_air
+						    && !was_gps_signal_lost
+						    && _ekfgsf_yaw_reset_count < _params.EKFGSF_reset_count_limit
+						    && isTimedOut(_ekfgsf_yaw_reset_time, 5'000'000)) {
+							// The minimum time interval between resets to the EKF-GSF estimate is limited to allow the EKF-GSF time
+							// to improve its estimate if the previous reset was not successful.
+							if (resetYawToEKFGSF()) {
+								ECL_WARN("GPS emergency yaw reset");
+							}
 						}
-					}
 
-					if (do_vel_pos_reset) {
 						ECL_WARN("GPS fusion timeout, resetting velocity and position");
 						_information_events.flags.reset_vel_to_gps = true;
 						_information_events.flags.reset_pos_to_gps = true;
@@ -220,20 +221,30 @@ bool Ekf::shouldResetGpsFusion() const
 	/* We are relying on aiding to constrain drift so after a specified time
 	 * with no aiding we need to do something
 	 */
-	const bool has_horizontal_aiding_timed_out = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
-						     && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
-						     && isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max);
-
-	const bool is_reset_required = has_horizontal_aiding_timed_out
+	const bool is_reset_required = hasHorizontalAidingTimedOut()
 				       || isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max);
 
+	/* Logic controlling the reset of navigation filter yaw to the EKF-GSF estimate to recover from loss of
+	 * navigation casued by a bad yaw estimate.
+
+	 * A rapid reset to the EKF-GSF estimate is performed after a recent takeoff if horizontal velocity
+	 * innovation checks fail. This enables recovery from a bad yaw estimate. After 30 seconds from takeoff,
+	 * different test criteria are used that take longer to trigger and reduce false positives. A reset is
+	 * not performed if the fault condition was present before flight to prevent triggering due to GPS glitches
+	 * or other sensor errors.
+	 */
+	const bool is_recent_takeoff_nav_failure = _control_status.flags.in_air
+			&& isRecent(_time_last_on_ground_us, 30000000)
+			&& isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay)
+			&& (_time_last_hor_vel_fuse > _time_last_on_ground_us);
+
 	const bool is_inflight_nav_failure = _control_status.flags.in_air
 					     && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
 					     && isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
 					     && (_time_last_hor_vel_fuse > _time_last_on_ground_us)
 					     && (_time_last_hor_pos_fuse > _time_last_on_ground_us);
 
-	return (is_reset_required || is_inflight_nav_failure);
+	return (is_reset_required || is_recent_takeoff_nav_failure || is_inflight_nav_failure);
 }
 
 bool Ekf::isYawFailure() const

src/modules/ekf2/EKF/mag_control.cpp

@@ -209,6 +209,7 @@ void Ekf::runInAirYawReset()
 			resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar());
 
 			_information_events.flags.yaw_aligned_to_imu_gps = true;
+			_ekfgsf_yaw_reset_time = _imu_sample_delayed.time_us;
 
 			// if world magnetic model (inclination, declination, strength) available then use it to reset mag states
 			if (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps)) {

src/modules/ekf2/EKF/optflow_fusion.cpp

@@ -45,8 +45,6 @@
 
 #include <mathlib/mathlib.h>
 #include <float.h>
-#include "python/ekf_derivation/generated/compute_flow_xy_innov_var_and_hx.h"
-#include "python/ekf_derivation/generated/compute_flow_y_innov_var_and_h.h"
 #include "utils.hpp"
 
 void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
@@ -54,8 +52,31 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
 	resetEstimatorAidStatus(aid_src);
 	aid_src.timestamp_sample = _flow_sample_delayed.time_us;
 
-	const Vector2f vel_body = predictFlowVelBody();
-	const float range = predictFlowRange();
+	// get rotation matrix from earth to body
+	const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal);
+
+	// calculate the sensor position relative to the IMU
+	const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
+
+	// calculate the velocity of the sensor relative to the imu in body frame
+	// Note: _flow_sample_delayed.gyro_xyz is the negative of the body angular velocity, thus use minus sign
+	const Vector3f vel_rel_imu_body = Vector3f(-_flow_sample_delayed.gyro_xyz / _flow_sample_delayed.dt) % pos_offset_body;
+
+	// calculate the velocity of the sensor in the earth frame
+	const Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body;
+
+	// rotate into body frame
+	const Vector3f vel_body = earth_to_body * vel_rel_earth;
+
+	// calculate the sensor position relative to the IMU in earth frame
+	const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
+
+	// calculate the height above the ground of the optical flow camera. Since earth frame is NED
+	// a positive offset in earth frame leads to a smaller height above the ground.
+	const float height_above_gnd_est = math::max(_terrain_vpos - _state.pos(2) - pos_offset_earth(2), fmaxf(_params.rng_gnd_clearance, 0.01f));
+
+	// calculate range from focal point to centre of image
+	const float range = height_above_gnd_est / earth_to_body(2, 2); // absolute distance to the frame region in view
 
 	// calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed
 	// correct for gyro bias errors in the data used to do the motion compensation
@@ -85,24 +106,150 @@ void Ekf::fuseOptFlow()
 
 	const float R_LOS = _aid_src_optical_flow.observation_variance[0];
 
+	// get latest estimated orientation
+	const float q0 = _state.quat_nominal(0);
+	const float q1 = _state.quat_nominal(1);
+	const float q2 = _state.quat_nominal(2);
+	const float q3 = _state.quat_nominal(3);
+
+	// get latest velocity in earth frame
+	const float vn = _state.vel(0);
+	const float ve = _state.vel(1);
+	const float vd = _state.vel(2);
+
 	// calculate the height above the ground of the optical flow camera. Since earth frame is NED
 	// a positive offset in earth frame leads to a smaller height above the ground.
-	float range = predictFlowRange();
+	const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
+	const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
+	const float height_above_gnd_est = math::max(_terrain_vpos - _state.pos(2) - pos_offset_earth(2), fmaxf(_params.rng_gnd_clearance, 0.01f));
 
-	const Vector24f state_vector = getStateAtFusionHorizonAsVector();
+	// calculate range from focal point to centre of image
+	const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal);
+	const float range = height_above_gnd_est / earth_to_body(2, 2); // absolute distance to the frame region in view
 
-	Vector2f innov_var;
-	Vector24f H;
-	sym::ComputeFlowXyInnovVarAndHx(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var, &H);
-	innov_var.copyTo(_aid_src_optical_flow.innovation_variance);
+	// The derivation allows for an arbitrary body to flow sensor frame rotation which is
+	// currently not supported by the EKF, so assume sensor frame is aligned with the
+	// body frame
+	const Dcmf Tbs = matrix::eye<float, 3>();
 
-	if ((_aid_src_optical_flow.innovation_variance[0] < R_LOS)
-	    || (_aid_src_optical_flow.innovation_variance[1] < R_LOS)) {
+	// Sub Expressions
+	const float HK0 = -Tbs(1,0)*q2 + Tbs(1,1)*q1 + Tbs(1,2)*q0;
+	const float HK1 = Tbs(1,0)*q3 + Tbs(1,1)*q0 - Tbs(1,2)*q1;
+	const float HK2 = Tbs(1,0)*q0 - Tbs(1,1)*q3 + Tbs(1,2)*q2;
+	const float HK3 = HK0*vd + HK1*ve + HK2*vn;
+	const float HK4 = 1.0F/range;
+	const float HK5 = 2*HK4;
+	const float HK6 = Tbs(1,0)*q1 + Tbs(1,1)*q2 + Tbs(1,2)*q3;
+	const float HK7 = -HK0*ve + HK1*vd + HK6*vn;
+	const float HK8 = HK0*vn - HK2*vd + HK6*ve;
+	const float HK9 = -HK1*vn + HK2*ve + HK6*vd;
+	const float HK10 = q0*q2;
+	const float HK11 = q1*q3;
+	const float HK12 = HK10 + HK11;
+	const float HK13 = 2*Tbs(1,2);
+	const float HK14 = q0*q3;
+	const float HK15 = q1*q2;
+	const float HK16 = HK14 - HK15;
+	const float HK17 = 2*Tbs(1,1);
+	const float HK18 = ecl::powf(q1, 2);
+	const float HK19 = ecl::powf(q2, 2);
+	const float HK20 = -HK19;
+	const float HK21 = ecl::powf(q0, 2);
+	const float HK22 = ecl::powf(q3, 2);
+	const float HK23 = HK21 - HK22;
+	const float HK24 = HK18 + HK20 + HK23;
+	const float HK25 = HK12*HK13 - HK16*HK17 + HK24*Tbs(1,0);
+	const float HK26 = HK14 + HK15;
+	const float HK27 = 2*Tbs(1,0);
+	const float HK28 = q0*q1;
+	const float HK29 = q2*q3;
+	const float HK30 = HK28 - HK29;
+	const float HK31 = -HK18;
+	const float HK32 = HK19 + HK23 + HK31;
+	const float HK33 = -HK13*HK30 + HK26*HK27 + HK32*Tbs(1,1);
+	const float HK34 = HK28 + HK29;
+	const float HK35 = HK10 - HK11;
+	const float HK36 = HK20 + HK21 + HK22 + HK31;
+	const float HK37 = HK17*HK34 - HK27*HK35 + HK36*Tbs(1,2);
+	const float HK38 = 2*HK3;
+	const float HK39 = 2*HK7;
+	const float HK40 = 2*HK8;
+	const float HK41 = 2*HK9;
+	const float HK42 = HK25*P(0,4) + HK33*P(0,5) + HK37*P(0,6) + HK38*P(0,0) + HK39*P(0,1) + HK40*P(0,2) + HK41*P(0,3);
+	const float HK43 = ecl::powf(range, -2);
+	const float HK44 = HK25*P(4,6) + HK33*P(5,6) + HK37*P(6,6) + HK38*P(0,6) + HK39*P(1,6) + HK40*P(2,6) + HK41*P(3,6);
+	const float HK45 = HK25*P(4,5) + HK33*P(5,5) + HK37*P(5,6) + HK38*P(0,5) + HK39*P(1,5) + HK40*P(2,5) + HK41*P(3,5);
+	const float HK46 = HK25*P(4,4) + HK33*P(4,5) + HK37*P(4,6) + HK38*P(0,4) + HK39*P(1,4) + HK40*P(2,4) + HK41*P(3,4);
+	const float HK47 = HK25*P(2,4) + HK33*P(2,5) + HK37*P(2,6) + HK38*P(0,2) + HK39*P(1,2) + HK40*P(2,2) + HK41*P(2,3);
+	const float HK48 = HK25*P(3,4) + HK33*P(3,5) + HK37*P(3,6) + HK38*P(0,3) + HK39*P(1,3) + HK40*P(2,3) + HK41*P(3,3);
+	const float HK49 = HK25*P(1,4) + HK33*P(1,5) + HK37*P(1,6) + HK38*P(0,1) + HK39*P(1,1) + HK40*P(1,2) + HK41*P(1,3);
+	// const float HK50 = HK4/(HK25*HK43*HK46 + HK33*HK43*HK45 + HK37*HK43*HK44 + HK38*HK42*HK43 + HK39*HK43*HK49 + HK40*HK43*HK47 + HK41*HK43*HK48 + R_LOS);
+
+	// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
+	_aid_src_optical_flow.innovation_variance[0] = (HK25*HK43*HK46 + HK33*HK43*HK45 + HK37*HK43*HK44 + HK38*HK42*HK43 + HK39*HK43*HK49 + HK40*HK43*HK47 + HK41*HK43*HK48 + R_LOS);
+
+	if (_aid_src_optical_flow.innovation_variance[0] < R_LOS) {
 		// we need to reinitialise the covariance matrix and abort this fusion step
-		ECL_ERR("Opt flow error - covariance reset");
 		initialiseCovariance();
 		return;
 	}
+	const float HK50 = HK4 / _aid_src_optical_flow.innovation_variance[0];
+
+	const float HK51 = Tbs(0,1)*q1;
+	const float HK52 = Tbs(0,2)*q0;
+	const float HK53 = Tbs(0,0)*q2;
+	const float HK54 = HK51 + HK52 - HK53;
+	const float HK55 = Tbs(0,0)*q3;
+	const float HK56 = Tbs(0,1)*q0;
+	const float HK57 = Tbs(0,2)*q1;
+	const float HK58 = HK55 + HK56 - HK57;
+	const float HK59 = Tbs(0,0)*q0;
+	const float HK60 = Tbs(0,2)*q2;
+	const float HK61 = Tbs(0,1)*q3;
+	const float HK62 = HK59 + HK60 - HK61;
+	const float HK63 = HK54*vd + HK58*ve + HK62*vn;
+	const float HK64 = Tbs(0,0)*q1 + Tbs(0,1)*q2 + Tbs(0,2)*q3;
+	const float HK65 = HK58*vd + HK64*vn;
+	const float HK66 = -HK54*ve + HK65;
+	const float HK67 = HK54*vn + HK64*ve;
+	const float HK68 = -HK62*vd + HK67;
+	const float HK69 = HK62*ve + HK64*vd;
+	const float HK70 = -HK58*vn + HK69;
+	const float HK71 = 2*Tbs(0,1);
+	const float HK72 = 2*Tbs(0,2);
+	const float HK73 = HK12*HK72 + HK24*Tbs(0,0);
+	const float HK74 = -HK16*HK71 + HK73;
+	const float HK75 = 2*Tbs(0,0);
+	const float HK76 = HK26*HK75 + HK32*Tbs(0,1);
+	const float HK77 = -HK30*HK72 + HK76;
+	const float HK78 = HK34*HK71 + HK36*Tbs(0,2);
+	const float HK79 = -HK35*HK75 + HK78;
+	const float HK80 = 2*HK63;
+	const float HK81 = 2*HK65 + 2*ve*(-HK51 - HK52 + HK53);
+	const float HK82 = 2*HK67 + 2*vd*(-HK59 - HK60 + HK61);
+	const float HK83 = 2*HK69 + 2*vn*(-HK55 - HK56 + HK57);
+	const float HK84 = HK71*(-HK14 + HK15) + HK73;
+	const float HK85 = HK72*(-HK28 + HK29) + HK76;
+	const float HK86 = HK75*(-HK10 + HK11) + HK78;
+	const float HK87 = HK80*P(0,0) + HK81*P(0,1) + HK82*P(0,2) + HK83*P(0,3) + HK84*P(0,4) + HK85*P(0,5) + HK86*P(0,6);
+	const float HK88 = HK80*P(0,6) + HK81*P(1,6) + HK82*P(2,6) + HK83*P(3,6) + HK84*P(4,6) + HK85*P(5,6) + HK86*P(6,6);
+	const float HK89 = HK80*P(0,5) + HK81*P(1,5) + HK82*P(2,5) + HK83*P(3,5) + HK84*P(4,5) + HK85*P(5,5) + HK86*P(5,6);
+	const float HK90 = HK80*P(0,4) + HK81*P(1,4) + HK82*P(2,4) + HK83*P(3,4) + HK84*P(4,4) + HK85*P(4,5) + HK86*P(4,6);
+	const float HK91 = HK80*P(0,2) + HK81*P(1,2) + HK82*P(2,2) + HK83*P(2,3) + HK84*P(2,4) + HK85*P(2,5) + HK86*P(2,6);
+	const float HK92 = 2*HK43;
+	const float HK93 = HK80*P(0,3) + HK81*P(1,3) + HK82*P(2,3) + HK83*P(3,3) + HK84*P(3,4) + HK85*P(3,5) + HK86*P(3,6);
+	const float HK94 = HK80*P(0,1) + HK81*P(1,1) + HK82*P(1,2) + HK83*P(1,3) + HK84*P(1,4) + HK85*P(1,5) + HK86*P(1,6);
+	// const float HK95 = HK4/(HK43*HK74*HK90 + HK43*HK77*HK89 + HK43*HK79*HK88 + HK43*HK80*HK87 + HK66*HK92*HK94 + HK68*HK91*HK92 + HK70*HK92*HK93 + R_LOS);
+
+	// calculate innovation variance for Y axis observation and protect against a badly conditioned calculation
+	_aid_src_optical_flow.innovation_variance[1] = (HK43*HK74*HK90 + HK43*HK77*HK89 + HK43*HK79*HK88 + HK43*HK80*HK87 + HK66*HK92*HK94 + HK68*HK91*HK92 + HK70*HK92*HK93 + R_LOS);
+	if (_aid_src_optical_flow.innovation_variance[1] < R_LOS) {
+		// we need to reinitialise the covariance matrix and abort this fusion step
+		initialiseCovariance();
+		return;
+	}
+
+	const float HK95 = HK4 / _aid_src_optical_flow.innovation_variance[1];
 
 	// run the innovation consistency check and record result
 	setEstimatorAidStatusTestRatio(_aid_src_optical_flow, math::max(_params.flow_innov_gate, 1.f));
@@ -118,37 +265,75 @@ void Ekf::fuseOptFlow()
 	bool fused[2] {false, false};
 
 	// fuse observation axes sequentially
-	for (uint8_t index = 0; index <= 1; index++) {
-		if (index == 0) {
-			// everything was already computed above
+	{
+		// Optical flow observation Jacobians - axis 0
+		SparseVector24f<0,1,2,3,4,5,6> Hfusion;
+		Hfusion.at<0>() = HK3*HK5;
+		Hfusion.at<1>() = HK5*HK7;
+		Hfusion.at<2>() = HK5*HK8;
+		Hfusion.at<3>() = HK5*HK9;
+		Hfusion.at<4>() = HK25*HK4;
+		Hfusion.at<5>() = HK33*HK4;
+		Hfusion.at<6>() = HK37*HK4;
 
-		} else if (index == 1) {
-			// recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes)
-			sym::ComputeFlowYInnovVarAndH(state_vector, P, range, R_LOS, FLT_EPSILON, &_aid_src_optical_flow.innovation_variance[1], &H);
+		// Optical flow Kalman gains - axis 0
+		Vector24f Kfusion;
+		Kfusion(0) = HK42*HK50;
+		Kfusion(1) = HK49*HK50;
+		Kfusion(2) = HK47*HK50;
+		Kfusion(3) = HK48*HK50;
+		Kfusion(4) = HK46*HK50;
+		Kfusion(5) = HK45*HK50;
+		Kfusion(6) = HK44*HK50;
 
-			// recalculate the innovation using the updated state
-			const Vector2f vel_body = predictFlowVelBody();
-			range = predictFlowRange();
-			_aid_src_optical_flow.innovation[1] = (-vel_body(0) / range) - _aid_src_optical_flow.observation[1];
-
-			if (_aid_src_optical_flow.innovation_variance[1] < R_LOS) {
-				// we need to reinitialise the covariance matrix and abort this fusion step
-				ECL_ERR("Opt flow error - covariance reset");
-				initialiseCovariance();
-				return;
-			}
+		for (unsigned row = 7; row <= 23; row++) {
+			Kfusion(row) = HK50*(HK25*P(4,row) + HK33*P(5,row) + HK37*P(6,row) + HK38*P(0,row) + HK39*P(1,row) + HK40*P(2,row) + HK41*P(3,row));
 		}
 
-		SparseVector24f<0,1,2,3,4,5,6> Hfusion(H);
-		Vector24f Kfusion = P * Hfusion / _aid_src_optical_flow.innovation_variance[index];
+		if (measurementUpdate(Kfusion, Hfusion, _aid_src_optical_flow.innovation[0])) {
+			fused[0] = true;
+			_fault_status.flags.bad_optflow_X = false;
 
-		if (measurementUpdate(Kfusion, Hfusion, _aid_src_optical_flow.innovation[index])) {
-			fused[index] = true;
+		} else {
+			_fault_status.flags.bad_optflow_X = true;
+			return;
 		}
 	}
 
-	_fault_status.flags.bad_optflow_X = !fused[0];
-	_fault_status.flags.bad_optflow_Y = !fused[1];
+	{
+		// Optical flow observation Jacobians - axis 1
+		SparseVector24f<0,1,2,3,4,5,6> Hfusion;
+		Hfusion.at<0>() = -HK5*HK63;
+		Hfusion.at<1>() = -HK5*HK66;
+		Hfusion.at<2>() = -HK5*HK68;
+		Hfusion.at<3>() = -HK5*HK70;
+		Hfusion.at<4>() = -HK4*HK74;
+		Hfusion.at<5>() = -HK4*HK77;
+		Hfusion.at<6>() = -HK4*HK79;
+
+		// Optical flow Kalman gains - axis 1
+		Vector24f Kfusion;
+		Kfusion(0) = -HK87*HK95;
+		Kfusion(1) = -HK94*HK95;
+		Kfusion(2) = -HK91*HK95;
+		Kfusion(3) = -HK93*HK95;
+		Kfusion(4) = -HK90*HK95;
+		Kfusion(5) = -HK89*HK95;
+		Kfusion(6) = -HK88*HK95;
+
+		for (unsigned row = 7; row <= 23; row++) {
+			Kfusion(row) = -HK95*(HK80*P(0,row) + HK81*P(1,row) + HK82*P(2,row) + HK83*P(3,row) + HK84*P(4,row) + HK85*P(5,row) + HK86*P(6,row));
+		}
+
+		if (measurementUpdate(Kfusion, Hfusion, _aid_src_optical_flow.innovation[1])) {
+			fused[1] = true;
+			_fault_status.flags.bad_optflow_Y = false;
+
+		} else {
+			_fault_status.flags.bad_optflow_Y = true;
+			return;
+		}
+	}
 
 	if (fused[0] && fused[1]) {
 		_aid_src_optical_flow.time_last_fuse = _imu_sample_delayed.time_us;
@@ -156,39 +341,6 @@ void Ekf::fuseOptFlow()
 	}
 }
 
-float Ekf::predictFlowRange()
-{
-	// calculate the sensor position relative to the IMU
-	const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
-
-	// calculate the sensor position relative to the IMU in earth frame
-	const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
-
-	// calculate the height above the ground of the optical flow camera. Since earth frame is NED
-	// a positive offset in earth frame leads to a smaller height above the ground.
-	const float height_above_gnd_est = math::max(_terrain_vpos - _state.pos(2) - pos_offset_earth(2), fmaxf(_params.rng_gnd_clearance, 0.01f));
-
-	// calculate range from focal point to centre of image
-	return height_above_gnd_est / _R_to_earth(2, 2); // absolute distance to the frame region in view
-}
-
-Vector2f Ekf::predictFlowVelBody()
-{
-	// calculate the sensor position relative to the IMU
-	const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
-
-	// calculate the velocity of the sensor relative to the imu in body frame
-	// Note: _flow_sample_delayed.gyro_xyz is the negative of the body angular velocity, thus use minus sign
-	const Vector3f vel_rel_imu_body = Vector3f(-_flow_sample_delayed.gyro_xyz / _flow_sample_delayed.dt) % pos_offset_body;
-
-	// calculate the velocity of the sensor in the earth frame
-	const Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body;
-
-	// rotate into body frame
-	return _state.quat_nominal.rotateVectorInverse(vel_rel_earth).xy();
-}
-
-
 // calculate optical flow body angular rate compensation
 // returns false if bias corrected body rate data is unavailable
 bool Ekf::calcOptFlowBodyRateComp()

src/modules/ekf2/EKF/python/ekf_derivation/derivation.py

@@ -337,56 +337,6 @@ def compute_mag_declination_innov_innov_var_and_h(
 
     return (innov, innov_var, H.T)
 
-def predict_opt_flow(state, distance, epsilon):
-    q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
-    R_to_earth = quat_to_rot(q_att)
-    R_to_body = R_to_earth.T
-
-    # Calculate earth relative velocity in a non-rotating sensor frame
-    v = sf.V3(state[State.vx], state[State.vy], state[State.vz])
-    rel_vel_sensor = R_to_body * v
-
-    # Divide by range to get predicted angular LOS rates relative to X and Y
-    # axes. Note these are rates in a non-rotating sensor frame
-    flow_pred = sf.V2()
-    flow_pred[0] =  rel_vel_sensor[1] / distance
-    flow_pred[1] = -rel_vel_sensor[0] / distance
-    flow_pred = add_epsilon_sign(flow_pred, distance, epsilon)
-
-    return flow_pred
-
-
-def compute_flow_xy_innov_var_and_hx(
-        state: VState,
-        P: MState,
-        distance: sf.Scalar,
-        R: sf.Scalar,
-        epsilon: sf.Scalar
-) -> (sf.V2, VState):
-    meas_pred = predict_opt_flow(state, distance, epsilon);
-
-    innov_var = sf.V2()
-    Hx = sf.V1(meas_pred[0]).jacobian(state)
-    innov_var[0] = (Hx * P * Hx.T + R)[0,0]
-    Hy = sf.V1(meas_pred[1]).jacobian(state)
-    innov_var[1] = (Hy * P * Hy.T + R)[0,0]
-
-    return (innov_var, Hx.T)
-
-def compute_flow_y_innov_var_and_h(
-        state: VState,
-        P: MState,
-        distance: sf.Scalar,
-        R: sf.Scalar,
-        epsilon: sf.Scalar
-) -> (sf.Scalar, VState):
-    meas_pred = predict_opt_flow(state, distance, epsilon);
-
-    Hy = sf.V1(meas_pred[1]).jacobian(state)
-    innov_var = (Hy * P * Hy.T + R)[0,0]
-
-    return (innov_var, Hy.T)
-
 print("Derive EKF2 equations...")
 generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"])
 generate_px4_function(compute_airspeed_h_and_k, output_names=["H", "K"])
@@ -402,5 +352,3 @@ generate_px4_function(compute_yaw_321_innov_var_and_h_alternate, output_names=["
 generate_px4_function(compute_yaw_312_innov_var_and_h, output_names=["innov_var", "H"])
 generate_px4_function(compute_yaw_312_innov_var_and_h_alternate, output_names=["innov_var", "H"])
 generate_px4_function(compute_mag_declination_innov_innov_var_and_h, output_names=["innov", "innov_var", "H"])
-generate_px4_function(compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"])
-generate_px4_function(compute_flow_y_innov_var_and_h, output_names=["innov_var", "H"])

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_flow_xy_innov_var_and_hx.h

@@ -1,127 +0,0 @@
-// -----------------------------------------------------------------------------
-// This file was autogenerated by symforce from template:
-//     backends/cpp/templates/function/FUNCTION.h.jinja
-// Do NOT modify by hand.
-// -----------------------------------------------------------------------------
-
-#pragma once
-
-#include <matrix/math.hpp>
-
-namespace sym {
-
-/**
- * This function was autogenerated from a symbolic function. Do not modify by hand.
- *
- * Symbolic function: compute_flow_xy_innov_var_and_hx
- *
- * Args:
- *     state: Matrix24_1
- *     P: Matrix24_24
- *     distance: Scalar
- *     R: Scalar
- *     epsilon: Scalar
- *
- * Outputs:
- *     innov_var: Matrix21
- *     H: Matrix24_1
- */
-template <typename Scalar>
-void ComputeFlowXyInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
-                                const matrix::Matrix<Scalar, 24, 24>& P, const Scalar distance,
-                                const Scalar R, const Scalar epsilon,
-                                matrix::Matrix<Scalar, 2, 1>* const innov_var = nullptr,
-                                matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
-  // Total ops: 285
-
-  // Input arrays
-
-  // Intermediate terms (29)
-  const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2));
-  const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2));
-  const Scalar _tmp2 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2));
-  const Scalar _tmp3 =
-      Scalar(1.0) /
-      (distance + epsilon * (2 * math::min<Scalar>(0, (((distance) > 0) - ((distance) < 0))) + 1));
-  const Scalar _tmp4 = _tmp3 * (_tmp0 - _tmp1 + _tmp2);
-  const Scalar _tmp5 = 2 * state(3, 0);
-  const Scalar _tmp6 = _tmp5 * state(0, 0);
-  const Scalar _tmp7 = 2 * state(2, 0);
-  const Scalar _tmp8 = _tmp7 * state(1, 0);
-  const Scalar _tmp9 = _tmp3 * (-_tmp6 + _tmp8);
-  const Scalar _tmp10 = 2 * state(4, 0);
-  const Scalar _tmp11 = _tmp10 * state(0, 0);
-  const Scalar _tmp12 = 2 * state(5, 0);
-  const Scalar _tmp13 = _tmp12 * state(3, 0);
-  const Scalar _tmp14 = _tmp7 * state(6, 0);
-  const Scalar _tmp15 = _tmp3 * (-_tmp11 - _tmp13 + _tmp14);
-  const Scalar _tmp16 = 2 * state(1, 0);
-  const Scalar _tmp17 =
-      _tmp3 * (-_tmp10 * state(3, 0) + _tmp12 * state(0, 0) + _tmp16 * state(6, 0));
-  const Scalar _tmp18 = _tmp7 * state(4, 0);
-  const Scalar _tmp19 = _tmp12 * state(1, 0);
-  const Scalar _tmp20 = 2 * state(0, 0) * state(6, 0);
-  const Scalar _tmp21 = _tmp3 * (_tmp18 - _tmp19 + _tmp20);
-  const Scalar _tmp22 = _tmp3 * (_tmp10 * state(1, 0) + _tmp5 * state(6, 0) + _tmp7 * state(5, 0));
-  const Scalar _tmp23 = _tmp3 * (_tmp16 * state(0, 0) + _tmp7 * state(3, 0));
-  const Scalar _tmp24 = _tmp3 * (-_tmp0 + _tmp1 + _tmp2);
-  const Scalar _tmp25 = _tmp3 * (_tmp6 + _tmp8);
-  const Scalar _tmp26 = _tmp3 * (_tmp16 * state(3, 0) - _tmp7 * state(0, 0));
-  const Scalar _tmp27 = _tmp3 * (_tmp11 + _tmp13 - _tmp14);
-  const Scalar _tmp28 = _tmp3 * (-_tmp18 + _tmp19 - _tmp20);
-
-  // Output terms (2)
-  if (innov_var != nullptr) {
-    matrix::Matrix<Scalar, 2, 1>& _innov_var = (*innov_var);
-
-    _innov_var(0, 0) =
-        R +
-        _tmp15 * (P(0, 3) * _tmp17 + P(1, 3) * _tmp21 + P(2, 3) * _tmp22 + P(3, 3) * _tmp15 +
-                  P(4, 3) * _tmp9 + P(5, 3) * _tmp4 + P(6, 3) * _tmp23) +
-        _tmp17 * (P(0, 0) * _tmp17 + P(1, 0) * _tmp21 + P(2, 0) * _tmp22 + P(3, 0) * _tmp15 +
-                  P(4, 0) * _tmp9 + P(5, 0) * _tmp4 + P(6, 0) * _tmp23) +
-        _tmp21 * (P(0, 1) * _tmp17 + P(1, 1) * _tmp21 + P(2, 1) * _tmp22 + P(3, 1) * _tmp15 +
-                  P(4, 1) * _tmp9 + P(5, 1) * _tmp4 + P(6, 1) * _tmp23) +
-        _tmp22 * (P(0, 2) * _tmp17 + P(1, 2) * _tmp21 + P(2, 2) * _tmp22 + P(3, 2) * _tmp15 +
-                  P(4, 2) * _tmp9 + P(5, 2) * _tmp4 + P(6, 2) * _tmp23) +
-        _tmp23 * (P(0, 6) * _tmp17 + P(1, 6) * _tmp21 + P(2, 6) * _tmp22 + P(3, 6) * _tmp15 +
-                  P(4, 6) * _tmp9 + P(5, 6) * _tmp4 + P(6, 6) * _tmp23) +
-        _tmp4 * (P(0, 5) * _tmp17 + P(1, 5) * _tmp21 + P(2, 5) * _tmp22 + P(3, 5) * _tmp15 +
-                 P(4, 5) * _tmp9 + P(5, 5) * _tmp4 + P(6, 5) * _tmp23) +
-        _tmp9 * (P(0, 4) * _tmp17 + P(1, 4) * _tmp21 + P(2, 4) * _tmp22 + P(3, 4) * _tmp15 +
-                 P(4, 4) * _tmp9 + P(5, 4) * _tmp4 + P(6, 4) * _tmp23);
-    _innov_var(1, 0) =
-        R -
-        _tmp17 * (-P(0, 3) * _tmp27 - P(1, 3) * _tmp22 - P(2, 3) * _tmp28 - P(3, 3) * _tmp17 -
-                  P(4, 3) * _tmp24 - P(5, 3) * _tmp25 - P(6, 3) * _tmp26) -
-        _tmp22 * (-P(0, 1) * _tmp27 - P(1, 1) * _tmp22 - P(2, 1) * _tmp28 - P(3, 1) * _tmp17 -
-                  P(4, 1) * _tmp24 - P(5, 1) * _tmp25 - P(6, 1) * _tmp26) -
-        _tmp24 * (-P(0, 4) * _tmp27 - P(1, 4) * _tmp22 - P(2, 4) * _tmp28 - P(3, 4) * _tmp17 -
-                  P(4, 4) * _tmp24 - P(5, 4) * _tmp25 - P(6, 4) * _tmp26) -
-        _tmp25 * (-P(0, 5) * _tmp27 - P(1, 5) * _tmp22 - P(2, 5) * _tmp28 - P(3, 5) * _tmp17 -
-                  P(4, 5) * _tmp24 - P(5, 5) * _tmp25 - P(6, 5) * _tmp26) -
-        _tmp26 * (-P(0, 6) * _tmp27 - P(1, 6) * _tmp22 - P(2, 6) * _tmp28 - P(3, 6) * _tmp17 -
-                  P(4, 6) * _tmp24 - P(5, 6) * _tmp25 - P(6, 6) * _tmp26) -
-        _tmp27 * (-P(0, 0) * _tmp27 - P(1, 0) * _tmp22 - P(2, 0) * _tmp28 - P(3, 0) * _tmp17 -
-                  P(4, 0) * _tmp24 - P(5, 0) * _tmp25 - P(6, 0) * _tmp26) -
-        _tmp28 * (-P(0, 2) * _tmp27 - P(1, 2) * _tmp22 - P(2, 2) * _tmp28 - P(3, 2) * _tmp17 -
-                  P(4, 2) * _tmp24 - P(5, 2) * _tmp25 - P(6, 2) * _tmp26);
-  }
-
-  if (H != nullptr) {
-    matrix::Matrix<Scalar, 24, 1>& _H = (*H);
-
-    _H.setZero();
-
-    _H(0, 0) = _tmp17;
-    _H(1, 0) = _tmp21;
-    _H(2, 0) = _tmp22;
-    _H(3, 0) = _tmp15;
-    _H(4, 0) = _tmp9;
-    _H(5, 0) = _tmp4;
-    _H(6, 0) = _tmp23;
-  }
-}  // NOLINT(readability/fn_size)
-
-// NOLINTNEXTLINE(readability/fn_size)
-}  // namespace sym

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_flow_y_innov_var_and_h.h

@@ -1,95 +0,0 @@
-// -----------------------------------------------------------------------------
-// This file was autogenerated by symforce from template:
-//     backends/cpp/templates/function/FUNCTION.h.jinja
-// Do NOT modify by hand.
-// -----------------------------------------------------------------------------
-
-#pragma once
-
-#include <matrix/math.hpp>
-
-namespace sym {
-
-/**
- * This function was autogenerated from a symbolic function. Do not modify by hand.
- *
- * Symbolic function: compute_flow_y_innov_var_and_h
- *
- * Args:
- *     state: Matrix24_1
- *     P: Matrix24_24
- *     distance: Scalar
- *     R: Scalar
- *     epsilon: Scalar
- *
- * Outputs:
- *     innov_var: Scalar
- *     H: Matrix24_1
- */
-template <typename Scalar>
-void ComputeFlowYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
-                              const matrix::Matrix<Scalar, 24, 24>& P, const Scalar distance,
-                              const Scalar R, const Scalar epsilon,
-                              Scalar* const innov_var = nullptr,
-                              matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
-  // Total ops: 171
-
-  // Input arrays
-
-  // Intermediate terms (13)
-  const Scalar _tmp0 =
-      Scalar(1.0) /
-      (distance + epsilon * (2 * math::min<Scalar>(0, (((distance) > 0) - ((distance) < 0))) + 1));
-  const Scalar _tmp1 =
-      _tmp0 * (std::pow(state(0, 0), Scalar(2)) + std::pow(state(1, 0), Scalar(2)) -
-               std::pow(state(2, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2)));
-  const Scalar _tmp2 = 2 * state(0, 0);
-  const Scalar _tmp3 = 2 * state(1, 0);
-  const Scalar _tmp4 = _tmp0 * (_tmp2 * state(3, 0) + _tmp3 * state(2, 0));
-  const Scalar _tmp5 = _tmp0 * (-_tmp2 * state(2, 0) + _tmp3 * state(3, 0));
-  const Scalar _tmp6 = 2 * state(4, 0);
-  const Scalar _tmp7 = 2 * state(6, 0);
-  const Scalar _tmp8 = _tmp0 * (_tmp2 * state(5, 0) - _tmp6 * state(3, 0) + _tmp7 * state(1, 0));
-  const Scalar _tmp9 = 2 * state(5, 0);
-  const Scalar _tmp10 = _tmp0 * (_tmp2 * state(4, 0) - _tmp7 * state(2, 0) + _tmp9 * state(3, 0));
-  const Scalar _tmp11 = _tmp0 * (_tmp3 * state(4, 0) + _tmp7 * state(3, 0) + _tmp9 * state(2, 0));
-  const Scalar _tmp12 = _tmp0 * (_tmp3 * state(5, 0) - _tmp6 * state(2, 0) - _tmp7 * state(0, 0));
-
-  // Output terms (2)
-  if (innov_var != nullptr) {
-    Scalar& _innov_var = (*innov_var);
-
-    _innov_var = R -
-                 _tmp1 * (-P(0, 4) * _tmp10 - P(1, 4) * _tmp11 - P(2, 4) * _tmp12 -
-                          P(3, 4) * _tmp8 - P(4, 4) * _tmp1 - P(5, 4) * _tmp4 - P(6, 4) * _tmp5) -
-                 _tmp10 * (-P(0, 0) * _tmp10 - P(1, 0) * _tmp11 - P(2, 0) * _tmp12 -
-                           P(3, 0) * _tmp8 - P(4, 0) * _tmp1 - P(5, 0) * _tmp4 - P(6, 0) * _tmp5) -
-                 _tmp11 * (-P(0, 1) * _tmp10 - P(1, 1) * _tmp11 - P(2, 1) * _tmp12 -
-                           P(3, 1) * _tmp8 - P(4, 1) * _tmp1 - P(5, 1) * _tmp4 - P(6, 1) * _tmp5) -
-                 _tmp12 * (-P(0, 2) * _tmp10 - P(1, 2) * _tmp11 - P(2, 2) * _tmp12 -
-                           P(3, 2) * _tmp8 - P(4, 2) * _tmp1 - P(5, 2) * _tmp4 - P(6, 2) * _tmp5) -
-                 _tmp4 * (-P(0, 5) * _tmp10 - P(1, 5) * _tmp11 - P(2, 5) * _tmp12 -
-                          P(3, 5) * _tmp8 - P(4, 5) * _tmp1 - P(5, 5) * _tmp4 - P(6, 5) * _tmp5) -
-                 _tmp5 * (-P(0, 6) * _tmp10 - P(1, 6) * _tmp11 - P(2, 6) * _tmp12 -
-                          P(3, 6) * _tmp8 - P(4, 6) * _tmp1 - P(5, 6) * _tmp4 - P(6, 6) * _tmp5) -
-                 _tmp8 * (-P(0, 3) * _tmp10 - P(1, 3) * _tmp11 - P(2, 3) * _tmp12 -
-                          P(3, 3) * _tmp8 - P(4, 3) * _tmp1 - P(5, 3) * _tmp4 - P(6, 3) * _tmp5);
-  }
-
-  if (H != nullptr) {
-    matrix::Matrix<Scalar, 24, 1>& _H = (*H);
-
-    _H.setZero();
-
-    _H(0, 0) = -_tmp10;
-    _H(1, 0) = -_tmp11;
-    _H(2, 0) = -_tmp12;
-    _H(3, 0) = -_tmp8;
-    _H(4, 0) = -_tmp1;
-    _H(5, 0) = -_tmp4;
-    _H(6, 0) = -_tmp5;
-  }
-}  // NOLINT(readability/fn_size)
-
-// NOLINTNEXTLINE(readability/fn_size)
-}  // namespace sym

src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_fusion_generated_compare.cpp

@@ -0,0 +1,640 @@
+#include <math.h>
+#include <stdio.h>
+#include <cstdlib>
+#include "../../../../../matrix/matrix/math.hpp"
+#include "util.h"
+
+typedef matrix::Vector<float, 24> Vector24f;
+typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
+
+template<int ... Idxs>
+using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
+
+int main()
+{
+    // Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations
+
+	SparseVector24f<0,1,2,3,4,5,6> Hfusion; // Optical flow observation Jacobians
+    Vector24f Kfusion; // Optical flow observation Kalman gains
+
+    Vector24f Hfusion_sympy;
+    Vector24f Kfusion_sympy;
+
+    Vector24f Hfusion_matlab;
+    Vector24f Kfusion_matlab;
+
+	const float R_LOS = sq(0.15f);
+
+    float flow_innov_var;
+
+    // quaternion inputs must be normalised
+    float q0 = 2.0f * ((float)rand() - 0.5f);
+    float q1 = 2.0f * ((float)rand() - 0.5f);
+    float q2 = 2.0f * ((float)rand() - 0.5f);
+    float q3 = 2.0f * ((float)rand() - 0.5f);
+    const float length = sqrtf(sq(q0) + sq(q1) + sq(q2) + sq(q3));
+    q0 /= length;
+    q1 /= length;
+    q2 /= length;
+    q3 /= length;
+
+	// get latest velocity in earth frame
+	const float vn = 10.0f * 2.0f * ((float)rand() - 0.5f);
+	const float ve = 10.0f * 2.0f * ((float)rand() - 0.5f);
+	const float vd = 2.0f * ((float)rand() - 0.5f);
+
+    const float range = 5.0f;
+
+    matrix::Dcmf Tbs;
+	Tbs.identity();
+
+    // create a symmetrical positive definite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1
+    SquareMatrix24f P;
+    for (int col=0; col<=23; col++) {
+        for (int row=0; row<=col; row++) {
+            if (row == col) {
+                P(row,col) = (float)rand();
+            } else {
+                P(col,row) = P(row,col) = 2.0f * ((float)rand() - 0.5f);
+            }
+        }
+    }
+
+    // evaluate sub expressions used by sympy code
+    const float HK0 = -Tbs(1,0)*q2 + Tbs(1,1)*q1 + Tbs(1,2)*q0;
+    const float HK1 = Tbs(1,0)*q3 + Tbs(1,1)*q0 - Tbs(1,2)*q1;
+    const float HK2 = Tbs(1,0)*q0 - Tbs(1,1)*q3 + Tbs(1,2)*q2;
+    const float HK3 = HK0*vd + HK1*ve + HK2*vn;
+    const float HK4 = 1.0F/range;
+    const float HK5 = 2*HK4;
+    const float HK6 = Tbs(1,0)*q1 + Tbs(1,1)*q2 + Tbs(1,2)*q3;
+    const float HK7 = -HK0*ve + HK1*vd + HK6*vn;
+    const float HK8 = HK0*vn - HK2*vd + HK6*ve;
+    const float HK9 = -HK1*vn + HK2*ve + HK6*vd;
+    const float HK10 = q0*q2;
+    const float HK11 = q1*q3;
+    const float HK12 = HK10 + HK11;
+    const float HK13 = 2*Tbs(1,2);
+    const float HK14 = q0*q3;
+    const float HK15 = q1*q2;
+    const float HK16 = HK14 - HK15;
+    const float HK17 = 2*Tbs(1,1);
+    const float HK18 = ecl::powf(q1, 2);
+    const float HK19 = ecl::powf(q2, 2);
+    const float HK20 = -HK19;
+    const float HK21 = ecl::powf(q0, 2);
+    const float HK22 = ecl::powf(q3, 2);
+    const float HK23 = HK21 - HK22;
+    const float HK24 = HK18 + HK20 + HK23;
+    const float HK25 = HK12*HK13 - HK16*HK17 + HK24*Tbs(1,0);
+    const float HK26 = HK14 + HK15;
+    const float HK27 = 2*Tbs(1,0);
+    const float HK28 = q0*q1;
+    const float HK29 = q2*q3;
+    const float HK30 = HK28 - HK29;
+    const float HK31 = -HK18;
+    const float HK32 = HK19 + HK23 + HK31;
+    const float HK33 = -HK13*HK30 + HK26*HK27 + HK32*Tbs(1,1);
+    const float HK34 = HK28 + HK29;
+    const float HK35 = HK10 - HK11;
+    const float HK36 = HK20 + HK21 + HK22 + HK31;
+    const float HK37 = HK17*HK34 - HK27*HK35 + HK36*Tbs(1,2);
+    const float HK38 = 2*HK3;
+    const float HK39 = 2*HK7;
+    const float HK40 = 2*HK8;
+    const float HK41 = 2*HK9;
+    const float HK42 = HK25*P(0,4) + HK33*P(0,5) + HK37*P(0,6) + HK38*P(0,0) + HK39*P(0,1) + HK40*P(0,2) + HK41*P(0,3);
+    const float HK43 = ecl::powf(range, -2);
+    const float HK44 = HK25*P(4,6) + HK33*P(5,6) + HK37*P(6,6) + HK38*P(0,6) + HK39*P(1,6) + HK40*P(2,6) + HK41*P(3,6);
+    const float HK45 = HK25*P(4,5) + HK33*P(5,5) + HK37*P(5,6) + HK38*P(0,5) + HK39*P(1,5) + HK40*P(2,5) + HK41*P(3,5);
+    const float HK46 = HK25*P(4,4) + HK33*P(4,5) + HK37*P(4,6) + HK38*P(0,4) + HK39*P(1,4) + HK40*P(2,4) + HK41*P(3,4);
+    const float HK47 = HK25*P(2,4) + HK33*P(2,5) + HK37*P(2,6) + HK38*P(0,2) + HK39*P(1,2) + HK40*P(2,2) + HK41*P(2,3);
+    const float HK48 = HK25*P(3,4) + HK33*P(3,5) + HK37*P(3,6) + HK38*P(0,3) + HK39*P(1,3) + HK40*P(2,3) + HK41*P(3,3);
+    const float HK49 = HK25*P(1,4) + HK33*P(1,5) + HK37*P(1,6) + HK38*P(0,1) + HK39*P(1,1) + HK40*P(1,2) + HK41*P(1,3);
+    const float HK50 = HK4/(HK25*HK43*HK46 + HK33*HK43*HK45 + HK37*HK43*HK44 + HK38*HK42*HK43 + HK39*HK43*HK49 + HK40*HK43*HK47 + HK41*HK43*HK48 + R_LOS);
+
+    const float HK51 = Tbs(0,1)*q1;
+    const float HK52 = Tbs(0,2)*q0;
+    const float HK53 = Tbs(0,0)*q2;
+    const float HK54 = HK51 + HK52 - HK53;
+    const float HK55 = Tbs(0,0)*q3;
+    const float HK56 = Tbs(0,1)*q0;
+    const float HK57 = Tbs(0,2)*q1;
+    const float HK58 = HK55 + HK56 - HK57;
+    const float HK59 = Tbs(0,0)*q0;
+    const float HK60 = Tbs(0,2)*q2;
+    const float HK61 = Tbs(0,1)*q3;
+    const float HK62 = HK59 + HK60 - HK61;
+    const float HK63 = HK54*vd + HK58*ve + HK62*vn;
+    const float HK64 = Tbs(0,0)*q1 + Tbs(0,1)*q2 + Tbs(0,2)*q3;
+    const float HK65 = HK58*vd + HK64*vn;
+    const float HK66 = -HK54*ve + HK65;
+    const float HK67 = HK54*vn + HK64*ve;
+    const float HK68 = -HK62*vd + HK67;
+    const float HK69 = HK62*ve + HK64*vd;
+    const float HK70 = -HK58*vn + HK69;
+    const float HK71 = 2*Tbs(0,1);
+    const float HK72 = 2*Tbs(0,2);
+    const float HK73 = HK12*HK72 + HK24*Tbs(0,0);
+    const float HK74 = -HK16*HK71 + HK73;
+    const float HK75 = 2*Tbs(0,0);
+    const float HK76 = HK26*HK75 + HK32*Tbs(0,1);
+    const float HK77 = -HK30*HK72 + HK76;
+    const float HK78 = HK34*HK71 + HK36*Tbs(0,2);
+    const float HK79 = -HK35*HK75 + HK78;
+    const float HK80 = 2*HK63;
+    const float HK81 = 2*HK65 + 2*ve*(-HK51 - HK52 + HK53);
+    const float HK82 = 2*HK67 + 2*vd*(-HK59 - HK60 + HK61);
+    const float HK83 = 2*HK69 + 2*vn*(-HK55 - HK56 + HK57);
+    const float HK84 = HK71*(-HK14 + HK15) + HK73;
+    const float HK85 = HK72*(-HK28 + HK29) + HK76;
+    const float HK86 = HK75*(-HK10 + HK11) + HK78;
+    const float HK87 = HK80*P(0,0) + HK81*P(0,1) + HK82*P(0,2) + HK83*P(0,3) + HK84*P(0,4) + HK85*P(0,5) + HK86*P(0,6);
+    const float HK88 = HK80*P(0,6) + HK81*P(1,6) + HK82*P(2,6) + HK83*P(3,6) + HK84*P(4,6) + HK85*P(5,6) + HK86*P(6,6);
+    const float HK89 = HK80*P(0,5) + HK81*P(1,5) + HK82*P(2,5) + HK83*P(3,5) + HK84*P(4,5) + HK85*P(5,5) + HK86*P(5,6);
+    const float HK90 = HK80*P(0,4) + HK81*P(1,4) + HK82*P(2,4) + HK83*P(3,4) + HK84*P(4,4) + HK85*P(4,5) + HK86*P(4,6);
+    const float HK91 = HK80*P(0,2) + HK81*P(1,2) + HK82*P(2,2) + HK83*P(2,3) + HK84*P(2,4) + HK85*P(2,5) + HK86*P(2,6);
+    const float HK92 = 2*HK43;
+    const float HK93 = HK80*P(0,3) + HK81*P(1,3) + HK82*P(2,3) + HK83*P(3,3) + HK84*P(3,4) + HK85*P(3,5) + HK86*P(3,6);
+    const float HK94 = HK80*P(0,1) + HK81*P(1,1) + HK82*P(1,2) + HK83*P(1,3) + HK84*P(1,4) + HK85*P(1,5) + HK86*P(1,6);
+    const float HK95 = HK4/(HK43*HK74*HK90 + HK43*HK77*HK89 + HK43*HK79*HK88 + HK43*HK80*HK87 + HK66*HK92*HK94 + HK68*HK91*HK92 + HK70*HK92*HK93 + R_LOS);
+
+
+    // Compare X axis equations
+    {
+        // evaluate sympy genrated equations for observatio Jacobians and Kalman gains
+        {
+            // calculate innovation variance for X axis observation and protect against a badly conditioned calculation
+            flow_innov_var = (HK25*HK43*HK46 + HK33*HK43*HK45 + HK37*HK43*HK44 + HK38*HK42*HK43 + HK39*HK43*HK49 + HK40*HK43*HK47 + HK41*HK43*HK48 + R_LOS);
+
+            const float HK50 = HK4/flow_innov_var;
+
+            // Observation Jacobians - axis 0
+            Hfusion.at<0>() = HK3*HK5;
+            Hfusion.at<1>() = HK5*HK7;
+            Hfusion.at<2>() = HK5*HK8;
+            Hfusion.at<3>() = HK5*HK9;
+            Hfusion.at<4>() = HK25*HK4;
+            Hfusion.at<5>() = HK33*HK4;
+            Hfusion.at<6>() = HK37*HK4;
+
+			// Kalman gains - axis 0
+			Kfusion(0) = HK42*HK50;
+			Kfusion(1) = HK49*HK50;
+			Kfusion(2) = HK47*HK50;
+			Kfusion(3) = HK48*HK50;
+			Kfusion(4) = HK46*HK50;
+			Kfusion(5) = HK45*HK50;
+			Kfusion(6) = HK44*HK50;
+
+			for (unsigned row = 7; row <= 23; row++) {
+				Kfusion(row) = HK50*(HK25*P(4,row) + HK33*P(5,row) + HK37*P(6,row) + HK38*P(0,row) + HK39*P(1,row) + HK40*P(2,row) + HK41*P(3,row));
+			}
+
+            // copy to arrays used for comparison
+            for (int row=0; row<7; row++) {
+                Hfusion_sympy(row) = Hfusion.atCompressedIndex(row);
+            }
+            for (int row=0; row<24; row++) {
+                Kfusion_sympy(row) = Kfusion(row);
+            }
+
+        }
+
+        // repeat calculation using matlab generated equations
+
+        {
+            // calculate X axis observation Jacobian
+            float t2 = 1.0f / range;
+            float H_LOS[24] = {};
+            H_LOS[0] = t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f);
+            H_LOS[1] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f);
+            H_LOS[2] = t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f);
+            H_LOS[3] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f);
+            H_LOS[4] = -t2*(q0*q3*2.0f-q1*q2*2.0f);
+            H_LOS[5] = t2*(q0*q0-q1*q1+q2*q2-q3*q3);
+            H_LOS[6] = t2*(q0*q1*2.0f+q2*q3*2.0f);
+
+            // calculate intermediate variables for the X observation innovatoin variance and Kalman gains
+            float t3 = q1*vd*2.0f;
+            float t4 = q0*ve*2.0f;
+            float t11 = q3*vn*2.0f;
+            float t5 = t3+t4-t11;
+            float t6 = q0*q3*2.0f;
+            float t29 = q1*q2*2.0f;
+            float t7 = t6-t29;
+            float t8 = q0*q1*2.0f;
+            float t9 = q2*q3*2.0f;
+            float t10 = t8+t9;
+            float t12 = P(0,0)*t2*t5;
+            float t13 = q0*vd*2.0f;
+            float t14 = q2*vn*2.0f;
+            float t28 = q1*ve*2.0f;
+            float t15 = t13+t14-t28;
+            float t16 = q3*vd*2.0f;
+            float t17 = q2*ve*2.0f;
+            float t18 = q1*vn*2.0f;
+            float t19 = t16+t17+t18;
+            float t20 = q3*ve*2.0f;
+            float t21 = q0*vn*2.0f;
+            float t30 = q2*vd*2.0f;
+            float t22 = t20+t21-t30;
+            float t23 = q0*q0;
+            float t24 = q1*q1;
+            float t25 = q2*q2;
+            float t26 = q3*q3;
+            float t27 = t23-t24+t25-t26;
+            float t31 = P(1,1)*t2*t15;
+            float t32 = P(6,0)*t2*t10;
+            float t33 = P(1,0)*t2*t15;
+            float t34 = P(2,0)*t2*t19;
+            float t35 = P(5,0)*t2*t27;
+            float t79 = P(4,0)*t2*t7;
+            float t80 = P(3,0)*t2*t22;
+            float t36 = t12+t32+t33+t34+t35-t79-t80;
+            float t37 = t2*t5*t36;
+            float t38 = P(6,1)*t2*t10;
+            float t39 = P(0,1)*t2*t5;
+            float t40 = P(2,1)*t2*t19;
+            float t41 = P(5,1)*t2*t27;
+            float t81 = P(4,1)*t2*t7;
+            float t82 = P(3,1)*t2*t22;
+            float t42 = t31+t38+t39+t40+t41-t81-t82;
+            float t43 = t2*t15*t42;
+            float t44 = P(6,2)*t2*t10;
+            float t45 = P(0,2)*t2*t5;
+            float t46 = P(1,2)*t2*t15;
+            float t47 = P(2,2)*t2*t19;
+            float t48 = P(5,2)*t2*t27;
+            float t83 = P(4,2)*t2*t7;
+            float t84 = P(3,2)*t2*t22;
+            float t49 = t44+t45+t46+t47+t48-t83-t84;
+            float t50 = t2*t19*t49;
+            float t51 = P(6,3)*t2*t10;
+            float t52 = P(0,3)*t2*t5;
+            float t53 = P(1,3)*t2*t15;
+            float t54 = P(2,3)*t2*t19;
+            float t55 = P(5,3)*t2*t27;
+            float t85 = P(4,3)*t2*t7;
+            float t86 = P(3,3)*t2*t22;
+            float t56 = t51+t52+t53+t54+t55-t85-t86;
+            float t57 = P(6,5)*t2*t10;
+            float t58 = P(0,5)*t2*t5;
+            float t59 = P(1,5)*t2*t15;
+            float t60 = P(2,5)*t2*t19;
+            float t61 = P(5,5)*t2*t27;
+            float t88 = P(4,5)*t2*t7;
+            float t89 = P(3,5)*t2*t22;
+            float t62 = t57+t58+t59+t60+t61-t88-t89;
+            float t63 = t2*t27*t62;
+            float t64 = P(6,4)*t2*t10;
+            float t65 = P(0,4)*t2*t5;
+            float t66 = P(1,4)*t2*t15;
+            float t67 = P(2,4)*t2*t19;
+            float t68 = P(5,4)*t2*t27;
+            float t90 = P(4,4)*t2*t7;
+            float t91 = P(3,4)*t2*t22;
+            float t69 = t64+t65+t66+t67+t68-t90-t91;
+            float t70 = P(6,6)*t2*t10;
+            float t71 = P(0,6)*t2*t5;
+            float t72 = P(1,6)*t2*t15;
+            float t73 = P(2,6)*t2*t19;
+            float t74 = P(5,6)*t2*t27;
+            float t93 = P(4,6)*t2*t7;
+            float t94 = P(3,6)*t2*t22;
+            float t75 = t70+t71+t72+t73+t74-t93-t94;
+            float t76 = t2*t10*t75;
+            float t87 = t2*t22*t56;
+            float t92 = t2*t7*t69;
+            float t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92;
+            float t78 = 1.0f / t77;
+            flow_innov_var = t77;
+
+            // calculate Kalman gains for X-axis observation
+            float Kfusion[24];
+            Kfusion[0] = t78*(t12-P(0,4)*t2*t7+P(0,1)*t2*t15+P(0,6)*t2*t10+P(0,2)*t2*t19-P(0,3)*t2*t22+P(0,5)*t2*t27);
+            Kfusion[1] = t78*(t31+P(1,0)*t2*t5-P(1,4)*t2*t7+P(1,6)*t2*t10+P(1,2)*t2*t19-P(1,3)*t2*t22+P(1,5)*t2*t27);
+            Kfusion[2] = t78*(t47+P(2,0)*t2*t5-P(2,4)*t2*t7+P(2,1)*t2*t15+P(2,6)*t2*t10-P(2,3)*t2*t22+P(2,5)*t2*t27);
+            Kfusion[3] = t78*(-t86+P(3,0)*t2*t5-P(3,4)*t2*t7+P(3,1)*t2*t15+P(3,6)*t2*t10+P(3,2)*t2*t19+P(3,5)*t2*t27);
+            Kfusion[4] = t78*(-t90+P(4,0)*t2*t5+P(4,1)*t2*t15+P(4,6)*t2*t10+P(4,2)*t2*t19-P(4,3)*t2*t22+P(4,5)*t2*t27);
+            Kfusion[5] = t78*(t61+P(5,0)*t2*t5-P(5,4)*t2*t7+P(5,1)*t2*t15+P(5,6)*t2*t10+P(5,2)*t2*t19-P(5,3)*t2*t22);
+            Kfusion[6] = t78*(t70+P(6,0)*t2*t5-P(6,4)*t2*t7+P(6,1)*t2*t15+P(6,2)*t2*t19-P(6,3)*t2*t22+P(6,5)*t2*t27);
+            Kfusion[7] = t78*(P(7,0)*t2*t5-P(7,4)*t2*t7+P(7,1)*t2*t15+P(7,6)*t2*t10+P(7,2)*t2*t19-P(7,3)*t2*t22+P(7,5)*t2*t27);
+            Kfusion[8] = t78*(P(8,0)*t2*t5-P(8,4)*t2*t7+P(8,1)*t2*t15+P(8,6)*t2*t10+P(8,2)*t2*t19-P(8,3)*t2*t22+P(8,5)*t2*t27);
+            Kfusion[9] = t78*(P(9,0)*t2*t5-P(9,4)*t2*t7+P(9,1)*t2*t15+P(9,6)*t2*t10+P(9,2)*t2*t19-P(9,3)*t2*t22+P(9,5)*t2*t27);
+            Kfusion[10] = t78*(P(10,0)*t2*t5-P(10,4)*t2*t7+P(10,1)*t2*t15+P(10,6)*t2*t10+P(10,2)*t2*t19-P(10,3)*t2*t22+P(10,5)*t2*t27);
+            Kfusion[11] = t78*(P(11,0)*t2*t5-P(11,4)*t2*t7+P(11,1)*t2*t15+P(11,6)*t2*t10+P(11,2)*t2*t19-P(11,3)*t2*t22+P(11,5)*t2*t27);
+            Kfusion[12] = t78*(P(12,0)*t2*t5-P(12,4)*t2*t7+P(12,1)*t2*t15+P(12,6)*t2*t10+P(12,2)*t2*t19-P(12,3)*t2*t22+P(12,5)*t2*t27);
+            Kfusion[13] = t78*(P(13,0)*t2*t5-P(13,4)*t2*t7+P(13,1)*t2*t15+P(13,6)*t2*t10+P(13,2)*t2*t19-P(13,3)*t2*t22+P(13,5)*t2*t27);
+            Kfusion[14] = t78*(P(14,0)*t2*t5-P(14,4)*t2*t7+P(14,1)*t2*t15+P(14,6)*t2*t10+P(14,2)*t2*t19-P(14,3)*t2*t22+P(14,5)*t2*t27);
+            Kfusion[15] = t78*(P(15,0)*t2*t5-P(15,4)*t2*t7+P(15,1)*t2*t15+P(15,6)*t2*t10+P(15,2)*t2*t19-P(15,3)*t2*t22+P(15,5)*t2*t27);
+            Kfusion[16] = t78*(P(16,0)*t2*t5-P(16,4)*t2*t7+P(16,1)*t2*t15+P(16,6)*t2*t10+P(16,2)*t2*t19-P(16,3)*t2*t22+P(16,5)*t2*t27);
+            Kfusion[17] = t78*(P(17,0)*t2*t5-P(17,4)*t2*t7+P(17,1)*t2*t15+P(17,6)*t2*t10+P(17,2)*t2*t19-P(17,3)*t2*t22+P(17,5)*t2*t27);
+            Kfusion[18] = t78*(P(18,0)*t2*t5-P(18,4)*t2*t7+P(18,1)*t2*t15+P(18,6)*t2*t10+P(18,2)*t2*t19-P(18,3)*t2*t22+P(18,5)*t2*t27);
+            Kfusion[19] = t78*(P(19,0)*t2*t5-P(19,4)*t2*t7+P(19,1)*t2*t15+P(19,6)*t2*t10+P(19,2)*t2*t19-P(19,3)*t2*t22+P(19,5)*t2*t27);
+            Kfusion[20] = t78*(P(20,0)*t2*t5-P(20,4)*t2*t7+P(20,1)*t2*t15+P(20,6)*t2*t10+P(20,2)*t2*t19-P(20,3)*t2*t22+P(20,5)*t2*t27);
+            Kfusion[21] = t78*(P(21,0)*t2*t5-P(21,4)*t2*t7+P(21,1)*t2*t15+P(21,6)*t2*t10+P(21,2)*t2*t19-P(21,3)*t2*t22+P(21,5)*t2*t27);
+            Kfusion[22] = t78*(P(22,0)*t2*t5-P(22,4)*t2*t7+P(22,1)*t2*t15+P(22,6)*t2*t10+P(22,2)*t2*t19-P(22,3)*t2*t22+P(22,5)*t2*t27);
+            Kfusion[23] = t78*(P(23,0)*t2*t5-P(23,4)*t2*t7+P(23,1)*t2*t15+P(23,6)*t2*t10+P(23,2)*t2*t19-P(23,3)*t2*t22+P(23,5)*t2*t27);
+
+            for (int row=0; row<24; row++) {
+                Hfusion_matlab(row) = H_LOS[row];
+                Kfusion_matlab(row) = Kfusion[row];
+            }
+        }
+
+        // find largest observation variance difference as a fraction of the matlab value
+        float max_diff_fraction = 0.0f;
+        int max_row;
+        float max_old, max_new;
+        for (int row=0; row<24; row++) {
+            float diff_fraction;
+            if (Hfusion_matlab(row) != 0.0f) {
+                diff_fraction = fabsf(Hfusion_sympy(row) - Hfusion_matlab(row)) / fabsf(Hfusion_matlab(row));
+            } else if (Hfusion_sympy(row) != 0.0f) {
+                diff_fraction = fabsf(Hfusion_sympy(row) - Hfusion_matlab(row)) / fabsf(Hfusion_sympy(row));
+            } else {
+                diff_fraction = 0.0f;
+            }
+            if (diff_fraction > max_diff_fraction) {
+                max_diff_fraction = diff_fraction;
+                max_row = row;
+                max_old = Hfusion_matlab(row);
+                max_new = Hfusion_sympy(row);
+            }
+        }
+
+        if (max_diff_fraction > 1e-5f) {
+            printf("Fail: Optical Flow X axis Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
+        } else {
+            printf("Pass: Optical Flow X axis Hfusion max diff fraction = %e\n",max_diff_fraction);
+        }
+
+        // find largest Kalman gain difference as a fraction of the matlab value
+        max_diff_fraction = 0.0f;
+        for (int row=0; row<24; row++) {
+            float diff_fraction;
+            if (Kfusion_matlab(row) != 0.0f) {
+                diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion_matlab(row)) / fabsf(Kfusion_matlab(row));
+            } else if (Hfusion_sympy(row) != 0.0f) {
+                diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion_matlab(row)) / fabsf(Kfusion_sympy(row));
+            } else {
+                diff_fraction = 0.0f;
+            }
+            if (diff_fraction > max_diff_fraction) {
+                max_diff_fraction = diff_fraction;
+                max_row = row;
+                max_old = Kfusion_matlab(row);
+                max_new = Kfusion_sympy(row);
+            }
+        }
+
+        if (max_diff_fraction > 1e-5f) {
+            printf("Fail: Optical Flow X axis Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
+        } else {
+            printf("Pass: Optical Flow X axis Kfusion max diff fraction = %e\n",max_diff_fraction);
+        }
+
+    }
+
+    // Compare Y axis equations
+
+    {
+        // evaluate sympy genrated equations for observatio Jacobians and Kalman gains
+        {
+            // calculate innovation variance for Y axis observation and protect against a badly conditioned calculation
+            flow_innov_var = (HK43*HK74*HK90 + HK43*HK77*HK89 + HK43*HK79*HK88 + HK43*HK80*HK87 + HK66*HK92*HK94 + HK68*HK91*HK92 + HK70*HK92*HK93 + R_LOS);
+
+            const float HK95 = HK4/flow_innov_var;
+
+			// Observation Jacobians - axis 1
+			Hfusion.at<0>() = -HK5*HK63;
+			Hfusion.at<1>() = -HK5*HK66;
+			Hfusion.at<2>() = -HK5*HK68;
+			Hfusion.at<3>() = -HK5*HK70;
+			Hfusion.at<4>() = -HK4*HK74;
+			Hfusion.at<5>() = -HK4*HK77;
+			Hfusion.at<6>() = -HK4*HK79;
+
+			// Kalman gains - axis 1
+			Kfusion(0) = -HK87*HK95;
+			Kfusion(1) = -HK94*HK95;
+			Kfusion(2) = -HK91*HK95;
+			Kfusion(3) = -HK93*HK95;
+			Kfusion(4) = -HK90*HK95;
+			Kfusion(5) = -HK89*HK95;
+			Kfusion(6) = -HK88*HK95;
+
+			for (unsigned row = 7; row <= 23; row++) {
+				Kfusion(row) = -HK95*(HK80*P(0,row) + HK81*P(1,row) + HK82*P(2,row) + HK83*P(3,row) + HK84*P(4,row) + HK85*P(5,row) + HK86*P(6,row));
+			}
+
+            // copy to arrays used for comparison
+            for (int row=0; row<7; row++) {
+                Hfusion_sympy(row) = Hfusion.atCompressedIndex(row);
+            }
+            for (int row=0; row<24; row++) {
+                Kfusion_sympy(row) = Kfusion(row);
+            }
+
+        }
+
+        // repeat calculation using matlab generated equations
+
+        {
+			// calculate Y axis observation Jacobian
+			float t2 = 1.0f / range;
+            float H_LOS[24] = {};
+			H_LOS[0] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f);
+			H_LOS[1] = -t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f);
+			H_LOS[2] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f);
+			H_LOS[3] = -t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f);
+			H_LOS[4] = -t2*(q0*q0+q1*q1-q2*q2-q3*q3);
+			H_LOS[5] = -t2*(q0*q3*2.0f+q1*q2*2.0f);
+			H_LOS[6] = t2*(q0*q2*2.0f-q1*q3*2.0f);
+
+			// calculate intermediate variables for the Y observation innovatoin variance and Kalman gains
+			float t3 = q3*ve*2.0f;
+			float t4 = q0*vn*2.0f;
+			float t11 = q2*vd*2.0f;
+			float t5 = t3+t4-t11;
+			float t6 = q0*q3*2.0f;
+			float t7 = q1*q2*2.0f;
+			float t8 = t6+t7;
+			float t9 = q0*q2*2.0f;
+			float t28 = q1*q3*2.0f;
+			float t10 = t9-t28;
+			float t12 = P(0,0)*t2*t5;
+			float t13 = q3*vd*2.0f;
+			float t14 = q2*ve*2.0f;
+			float t15 = q1*vn*2.0f;
+			float t16 = t13+t14+t15;
+			float t17 = q0*vd*2.0f;
+			float t18 = q2*vn*2.0f;
+			float t29 = q1*ve*2.0f;
+			float t19 = t17+t18-t29;
+			float t20 = q1*vd*2.0f;
+			float t21 = q0*ve*2.0f;
+			float t30 = q3*vn*2.0f;
+			float t22 = t20+t21-t30;
+			float t23 = q0*q0;
+			float t24 = q1*q1;
+			float t25 = q2*q2;
+			float t26 = q3*q3;
+			float t27 = t23+t24-t25-t26;
+			float t31 = P(1,1)*t2*t16;
+			float t32 = P(5,0)*t2*t8;
+			float t33 = P(1,0)*t2*t16;
+			float t34 = P(3,0)*t2*t22;
+			float t35 = P(4,0)*t2*t27;
+			float t80 = P(6,0)*t2*t10;
+			float t81 = P(2,0)*t2*t19;
+			float t36 = t12+t32+t33+t34+t35-t80-t81;
+			float t37 = t2*t5*t36;
+			float t38 = P(5,1)*t2*t8;
+			float t39 = P(0,1)*t2*t5;
+			float t40 = P(3,1)*t2*t22;
+			float t41 = P(4,1)*t2*t27;
+			float t82 = P(6,1)*t2*t10;
+			float t83 = P(2,1)*t2*t19;
+			float t42 = t31+t38+t39+t40+t41-t82-t83;
+			float t43 = t2*t16*t42;
+			float t44 = P(5,2)*t2*t8;
+			float t45 = P(0,2)*t2*t5;
+			float t46 = P(1,2)*t2*t16;
+			float t47 = P(3,2)*t2*t22;
+			float t48 = P(4,2)*t2*t27;
+			float t79 = P(2,2)*t2*t19;
+			float t84 = P(6,2)*t2*t10;
+			float t49 = t44+t45+t46+t47+t48-t79-t84;
+			float t50 = P(5,3)*t2*t8;
+			float t51 = P(0,3)*t2*t5;
+			float t52 = P(1,3)*t2*t16;
+			float t53 = P(3,3)*t2*t22;
+			float t54 = P(4,3)*t2*t27;
+			float t86 = P(6,3)*t2*t10;
+			float t87 = P(2,3)*t2*t19;
+			float t55 = t50+t51+t52+t53+t54-t86-t87;
+			float t56 = t2*t22*t55;
+			float t57 = P(5,4)*t2*t8;
+			float t58 = P(0,4)*t2*t5;
+			float t59 = P(1,4)*t2*t16;
+			float t60 = P(3,4)*t2*t22;
+			float t61 = P(4,4)*t2*t27;
+			float t88 = P(6,4)*t2*t10;
+			float t89 = P(2,4)*t2*t19;
+			float t62 = t57+t58+t59+t60+t61-t88-t89;
+			float t63 = t2*t27*t62;
+			float t64 = P(5,5)*t2*t8;
+			float t65 = P(0,5)*t2*t5;
+			float t66 = P(1,5)*t2*t16;
+			float t67 = P(3,5)*t2*t22;
+			float t68 = P(4,5)*t2*t27;
+			float t90 = P(6,5)*t2*t10;
+			float t91 = P(2,5)*t2*t19;
+			float t69 = t64+t65+t66+t67+t68-t90-t91;
+			float t70 = t2*t8*t69;
+			float t71 = P(5,6)*t2*t8;
+			float t72 = P(0,6)*t2*t5;
+			float t73 = P(1,6)*t2*t16;
+			float t74 = P(3,6)*t2*t22;
+			float t75 = P(4,6)*t2*t27;
+			float t92 = P(6,6)*t2*t10;
+			float t93 = P(2,6)*t2*t19;
+			float t76 = t71+t72+t73+t74+t75-t92-t93;
+			float t85 = t2*t19*t49;
+			float t94 = t2*t10*t76;
+			float t77 = R_LOS+t37+t43+t56+t63+t70-t85-t94;
+
+			float t78 = 1.0f / t77;
+			flow_innov_var = t77;
+
+			// calculate Kalman gains for Y-axis observation
+            float Kfusion[24];
+			Kfusion[0] = -t78*(t12+P(0,5)*t2*t8-P(0,6)*t2*t10+P(0,1)*t2*t16-P(0,2)*t2*t19+P(0,3)*t2*t22+P(0,4)*t2*t27);
+			Kfusion[1] = -t78*(t31+P(1,0)*t2*t5+P(1,5)*t2*t8-P(1,6)*t2*t10-P(1,2)*t2*t19+P(1,3)*t2*t22+P(1,4)*t2*t27);
+			Kfusion[2] = -t78*(-t79+P(2,0)*t2*t5+P(2,5)*t2*t8-P(2,6)*t2*t10+P(2,1)*t2*t16+P(2,3)*t2*t22+P(2,4)*t2*t27);
+			Kfusion[3] = -t78*(t53+P(3,0)*t2*t5+P(3,5)*t2*t8-P(3,6)*t2*t10+P(3,1)*t2*t16-P(3,2)*t2*t19+P(3,4)*t2*t27);
+			Kfusion[4] = -t78*(t61+P(4,0)*t2*t5+P(4,5)*t2*t8-P(4,6)*t2*t10+P(4,1)*t2*t16-P(4,2)*t2*t19+P(4,3)*t2*t22);
+			Kfusion[5] = -t78*(t64+P(5,0)*t2*t5-P(5,6)*t2*t10+P(5,1)*t2*t16-P(5,2)*t2*t19+P(5,3)*t2*t22+P(5,4)*t2*t27);
+			Kfusion[6] = -t78*(-t92+P(6,0)*t2*t5+P(6,5)*t2*t8+P(6,1)*t2*t16-P(6,2)*t2*t19+P(6,3)*t2*t22+P(6,4)*t2*t27);
+			Kfusion[7] = -t78*(P(7,0)*t2*t5+P(7,5)*t2*t8-P(7,6)*t2*t10+P(7,1)*t2*t16-P(7,2)*t2*t19+P(7,3)*t2*t22+P(7,4)*t2*t27);
+			Kfusion[8] = -t78*(P(8,0)*t2*t5+P(8,5)*t2*t8-P(8,6)*t2*t10+P(8,1)*t2*t16-P(8,2)*t2*t19+P(8,3)*t2*t22+P(8,4)*t2*t27);
+			Kfusion[9] = -t78*(P(9,0)*t2*t5+P(9,5)*t2*t8-P(9,6)*t2*t10+P(9,1)*t2*t16-P(9,2)*t2*t19+P(9,3)*t2*t22+P(9,4)*t2*t27);
+			Kfusion[10] = -t78*(P(10,0)*t2*t5+P(10,5)*t2*t8-P(10,6)*t2*t10+P(10,1)*t2*t16-P(10,2)*t2*t19+P(10,3)*t2*t22+P(10,4)*t2*t27);
+			Kfusion[11] = -t78*(P(11,0)*t2*t5+P(11,5)*t2*t8-P(11,6)*t2*t10+P(11,1)*t2*t16-P(11,2)*t2*t19+P(11,3)*t2*t22+P(11,4)*t2*t27);
+			Kfusion[12] = -t78*(P(12,0)*t2*t5+P(12,5)*t2*t8-P(12,6)*t2*t10+P(12,1)*t2*t16-P(12,2)*t2*t19+P(12,3)*t2*t22+P(12,4)*t2*t27);
+			Kfusion[13] = -t78*(P(13,0)*t2*t5+P(13,5)*t2*t8-P(13,6)*t2*t10+P(13,1)*t2*t16-P(13,2)*t2*t19+P(13,3)*t2*t22+P(13,4)*t2*t27);
+			Kfusion[14] = -t78*(P(14,0)*t2*t5+P(14,5)*t2*t8-P(14,6)*t2*t10+P(14,1)*t2*t16-P(14,2)*t2*t19+P(14,3)*t2*t22+P(14,4)*t2*t27);
+			Kfusion[15] = -t78*(P(15,0)*t2*t5+P(15,5)*t2*t8-P(15,6)*t2*t10+P(15,1)*t2*t16-P(15,2)*t2*t19+P(15,3)*t2*t22+P(15,4)*t2*t27);
+			Kfusion[16] = -t78*(P(16,0)*t2*t5+P(16,5)*t2*t8-P(16,6)*t2*t10+P(16,1)*t2*t16-P(16,2)*t2*t19+P(16,3)*t2*t22+P(16,4)*t2*t27);
+			Kfusion[17] = -t78*(P(17,0)*t2*t5+P(17,5)*t2*t8-P(17,6)*t2*t10+P(17,1)*t2*t16-P(17,2)*t2*t19+P(17,3)*t2*t22+P(17,4)*t2*t27);
+			Kfusion[18] = -t78*(P(18,0)*t2*t5+P(18,5)*t2*t8-P(18,6)*t2*t10+P(18,1)*t2*t16-P(18,2)*t2*t19+P(18,3)*t2*t22+P(18,4)*t2*t27);
+			Kfusion[19] = -t78*(P(19,0)*t2*t5+P(19,5)*t2*t8-P(19,6)*t2*t10+P(19,1)*t2*t16-P(19,2)*t2*t19+P(19,3)*t2*t22+P(19,4)*t2*t27);
+			Kfusion[20] = -t78*(P(20,0)*t2*t5+P(20,5)*t2*t8-P(20,6)*t2*t10+P(20,1)*t2*t16-P(20,2)*t2*t19+P(20,3)*t2*t22+P(20,4)*t2*t27);
+			Kfusion[21] = -t78*(P(21,0)*t2*t5+P(21,5)*t2*t8-P(21,6)*t2*t10+P(21,1)*t2*t16-P(21,2)*t2*t19+P(21,3)*t2*t22+P(21,4)*t2*t27);
+			Kfusion[22] = -t78*(P(22,0)*t2*t5+P(22,5)*t2*t8-P(22,6)*t2*t10+P(22,1)*t2*t16-P(22,2)*t2*t19+P(22,3)*t2*t22+P(22,4)*t2*t27);
+			Kfusion[23] = -t78*(P(23,0)*t2*t5+P(23,5)*t2*t8-P(23,6)*t2*t10+P(23,1)*t2*t16-P(23,2)*t2*t19+P(23,3)*t2*t22+P(23,4)*t2*t27);
+
+            for (int row=0; row<24; row++) {
+                Hfusion_matlab(row) = H_LOS[row];
+                Kfusion_matlab(row) = Kfusion[row];
+            }
+        }
+
+        // find largest observation variance difference as a fraction of the matlab value
+        float max_diff_fraction = 0.0f;
+        int max_row;
+        float max_old, max_new;
+        for (int row=0; row<24; row++) {
+            float diff_fraction;
+            if (Hfusion_matlab(row) != 0.0f) {
+                diff_fraction = fabsf(Hfusion_sympy(row) - Hfusion_matlab(row)) / fabsf(Hfusion_matlab(row));
+            } else if (Hfusion_sympy(row) != 0.0f) {
+                diff_fraction = fabsf(Hfusion_sympy(row) - Hfusion_matlab(row)) / fabsf(Hfusion_sympy(row));
+            } else {
+                diff_fraction = 0.0f;
+            }
+            if (diff_fraction > max_diff_fraction) {
+                max_diff_fraction = diff_fraction;
+                max_row = row;
+                max_old = Hfusion_matlab(row);
+                max_new = Hfusion_sympy(row);
+            }
+        }
+
+        if (max_diff_fraction > 1e-5f) {
+            printf("Fail: Optical Flow Y axis Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
+        } else {
+            printf("Pass: Optical Flow Y axis Hfusion max diff fraction = %e\n",max_diff_fraction);
+        }
+
+        // find largest Kalman gain difference as a fraction of the matlab value
+        max_diff_fraction = 0.0f;
+        for (int row=0; row<24; row++) {
+            float diff_fraction;
+            if (Kfusion_matlab(row) != 0.0f) {
+                diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion_matlab(row)) / fabsf(Kfusion_matlab(row));
+            } else if (Hfusion_sympy(row) != 0.0f) {
+                diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion_matlab(row)) / fabsf(Kfusion_sympy(row));
+            } else {
+                diff_fraction = 0.0f;
+            }
+            if (diff_fraction > max_diff_fraction) {
+                max_diff_fraction = diff_fraction;
+                max_row = row;
+                max_old = Kfusion_matlab(row);
+                max_new = Kfusion_sympy(row);
+            }
+        }
+
+        if (max_diff_fraction > 1e-5f) {
+            printf("Fail: Optical Flow Y axis Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
+        } else {
+            printf("Pass: Optical Flow Y axis Kfusion max diff fraction = %e\n",max_diff_fraction);
+        }
+
+    }
+
+    return 0;
+}

src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated.cpp

@@ -0,0 +1,184 @@
+// X Axis Equations
+// Sub Expressions
+const float HK0 = -Tbs(1,0)*q2 + Tbs(1,1)*q1 + Tbs(1,2)*q0;
+const float HK1 = Tbs(1,0)*q3 + Tbs(1,1)*q0 - Tbs(1,2)*q1;
+const float HK2 = Tbs(1,0)*q0 - Tbs(1,1)*q3 + Tbs(1,2)*q2;
+const float HK3 = HK0*vd + HK1*ve + HK2*vn;
+const float HK4 = 1.0F/(range);
+const float HK5 = 2*HK4;
+const float HK6 = Tbs(1,0)*q1 + Tbs(1,1)*q2 + Tbs(1,2)*q3;
+const float HK7 = -HK0*ve + HK1*vd + HK6*vn;
+const float HK8 = HK0*vn - HK2*vd + HK6*ve;
+const float HK9 = -HK1*vn + HK2*ve + HK6*vd;
+const float HK10 = q0*q2;
+const float HK11 = q1*q3;
+const float HK12 = 2*Tbs(1,2);
+const float HK13 = q0*q3;
+const float HK14 = q1*q2;
+const float HK15 = 2*Tbs(1,1);
+const float HK16 = (q1)*(q1);
+const float HK17 = (q2)*(q2);
+const float HK18 = -HK17;
+const float HK19 = (q0)*(q0);
+const float HK20 = (q3)*(q3);
+const float HK21 = HK19 - HK20;
+const float HK22 = HK12*(HK10 + HK11) - HK15*(HK13 - HK14) + Tbs(1,0)*(HK16 + HK18 + HK21);
+const float HK23 = 2*Tbs(1,0);
+const float HK24 = q0*q1;
+const float HK25 = q2*q3;
+const float HK26 = -HK16;
+const float HK27 = -HK12*(HK24 - HK25) + HK23*(HK13 + HK14) + Tbs(1,1)*(HK17 + HK21 + HK26);
+const float HK28 = HK15*(HK24 + HK25) - HK23*(HK10 - HK11) + Tbs(1,2)*(HK18 + HK19 + HK20 + HK26);
+const float HK29 = 2*HK3;
+const float HK30 = 2*HK7;
+const float HK31 = 2*HK8;
+const float HK32 = 2*HK9;
+const float HK33 = HK22*P(0,4) + HK27*P(0,5) + HK28*P(0,6) + HK29*P(0,0) + HK30*P(0,1) + HK31*P(0,2) + HK32*P(0,3);
+const float HK34 = 1.0F/((range)*(range));
+const float HK35 = HK22*P(4,6) + HK27*P(5,6) + HK28*P(6,6) + HK29*P(0,6) + HK30*P(1,6) + HK31*P(2,6) + HK32*P(3,6);
+const float HK36 = HK22*P(4,5) + HK27*P(5,5) + HK28*P(5,6) + HK29*P(0,5) + HK30*P(1,5) + HK31*P(2,5) + HK32*P(3,5);
+const float HK37 = HK22*P(4,4) + HK27*P(4,5) + HK28*P(4,6) + HK29*P(0,4) + HK30*P(1,4) + HK31*P(2,4) + HK32*P(3,4);
+const float HK38 = HK22*P(2,4) + HK27*P(2,5) + HK28*P(2,6) + HK29*P(0,2) + HK30*P(1,2) + HK31*P(2,2) + HK32*P(2,3);
+const float HK39 = HK22*P(3,4) + HK27*P(3,5) + HK28*P(3,6) + HK29*P(0,3) + HK30*P(1,3) + HK31*P(2,3) + HK32*P(3,3);
+const float HK40 = HK22*P(1,4) + HK27*P(1,5) + HK28*P(1,6) + HK29*P(0,1) + HK30*P(1,1) + HK31*P(1,2) + HK32*P(1,3);
+const float HK41 = HK4/(HK22*HK34*HK37 + HK27*HK34*HK36 + HK28*HK34*HK35 + HK29*HK33*HK34 + HK30*HK34*HK40 + HK31*HK34*HK38 + HK32*HK34*HK39 + R_LOS);
+
+
+// Observation Jacobians
+Hfusion.at<0>() = HK3*HK5;
+Hfusion.at<1>() = HK5*HK7;
+Hfusion.at<2>() = HK5*HK8;
+Hfusion.at<3>() = HK5*HK9;
+Hfusion.at<4>() = HK22*HK4;
+Hfusion.at<5>() = HK27*HK4;
+Hfusion.at<6>() = HK28*HK4;
+
+
+// Kalman gains
+Kfusion(0) = HK33*HK41;
+Kfusion(1) = HK40*HK41;
+Kfusion(2) = HK38*HK41;
+Kfusion(3) = HK39*HK41;
+Kfusion(4) = HK37*HK41;
+Kfusion(5) = HK36*HK41;
+Kfusion(6) = HK35*HK41;
+Kfusion(7) = HK41*(HK22*P(4,7) + HK27*P(5,7) + HK28*P(6,7) + HK29*P(0,7) + HK30*P(1,7) + HK31*P(2,7) + HK32*P(3,7));
+Kfusion(8) = HK41*(HK22*P(4,8) + HK27*P(5,8) + HK28*P(6,8) + HK29*P(0,8) + HK30*P(1,8) + HK31*P(2,8) + HK32*P(3,8));
+Kfusion(9) = HK41*(HK22*P(4,9) + HK27*P(5,9) + HK28*P(6,9) + HK29*P(0,9) + HK30*P(1,9) + HK31*P(2,9) + HK32*P(3,9));
+Kfusion(10) = HK41*(HK22*P(4,10) + HK27*P(5,10) + HK28*P(6,10) + HK29*P(0,10) + HK30*P(1,10) + HK31*P(2,10) + HK32*P(3,10));
+Kfusion(11) = HK41*(HK22*P(4,11) + HK27*P(5,11) + HK28*P(6,11) + HK29*P(0,11) + HK30*P(1,11) + HK31*P(2,11) + HK32*P(3,11));
+Kfusion(12) = HK41*(HK22*P(4,12) + HK27*P(5,12) + HK28*P(6,12) + HK29*P(0,12) + HK30*P(1,12) + HK31*P(2,12) + HK32*P(3,12));
+Kfusion(13) = HK41*(HK22*P(4,13) + HK27*P(5,13) + HK28*P(6,13) + HK29*P(0,13) + HK30*P(1,13) + HK31*P(2,13) + HK32*P(3,13));
+Kfusion(14) = HK41*(HK22*P(4,14) + HK27*P(5,14) + HK28*P(6,14) + HK29*P(0,14) + HK30*P(1,14) + HK31*P(2,14) + HK32*P(3,14));
+Kfusion(15) = HK41*(HK22*P(4,15) + HK27*P(5,15) + HK28*P(6,15) + HK29*P(0,15) + HK30*P(1,15) + HK31*P(2,15) + HK32*P(3,15));
+Kfusion(16) = HK41*(HK22*P(4,16) + HK27*P(5,16) + HK28*P(6,16) + HK29*P(0,16) + HK30*P(1,16) + HK31*P(2,16) + HK32*P(3,16));
+Kfusion(17) = HK41*(HK22*P(4,17) + HK27*P(5,17) + HK28*P(6,17) + HK29*P(0,17) + HK30*P(1,17) + HK31*P(2,17) + HK32*P(3,17));
+Kfusion(18) = HK41*(HK22*P(4,18) + HK27*P(5,18) + HK28*P(6,18) + HK29*P(0,18) + HK30*P(1,18) + HK31*P(2,18) + HK32*P(3,18));
+Kfusion(19) = HK41*(HK22*P(4,19) + HK27*P(5,19) + HK28*P(6,19) + HK29*P(0,19) + HK30*P(1,19) + HK31*P(2,19) + HK32*P(3,19));
+Kfusion(20) = HK41*(HK22*P(4,20) + HK27*P(5,20) + HK28*P(6,20) + HK29*P(0,20) + HK30*P(1,20) + HK31*P(2,20) + HK32*P(3,20));
+Kfusion(21) = HK41*(HK22*P(4,21) + HK27*P(5,21) + HK28*P(6,21) + HK29*P(0,21) + HK30*P(1,21) + HK31*P(2,21) + HK32*P(3,21));
+Kfusion(22) = HK41*(HK22*P(4,22) + HK27*P(5,22) + HK28*P(6,22) + HK29*P(0,22) + HK30*P(1,22) + HK31*P(2,22) + HK32*P(3,22));
+Kfusion(23) = HK41*(HK22*P(4,23) + HK27*P(5,23) + HK28*P(6,23) + HK29*P(0,23) + HK30*P(1,23) + HK31*P(2,23) + HK32*P(3,23));
+
+
+// Predicted observation
+
+
+// Innovation variance
+
+
+// Y Axis Equations
+// Sub Expressions
+const float HK0 = -Tbs(0,0)*q2 + Tbs(0,1)*q1 + Tbs(0,2)*q0;
+const float HK1 = Tbs(0,0)*q3 + Tbs(0,1)*q0 - Tbs(0,2)*q1;
+const float HK2 = Tbs(0,0)*q0 - Tbs(0,1)*q3 + Tbs(0,2)*q2;
+const float HK3 = HK0*vd + HK1*ve + HK2*vn;
+const float HK4 = 1.0F/(range);
+const float HK5 = 2*HK4;
+const float HK6 = Tbs(0,0)*q1 + Tbs(0,1)*q2 + Tbs(0,2)*q3;
+const float HK7 = HK1*vd + HK6*vn;
+const float HK8 = HK0*vn + HK6*ve;
+const float HK9 = HK2*ve + HK6*vd;
+const float HK10 = q0*q3;
+const float HK11 = q1*q2;
+const float HK12 = HK10 - HK11;
+const float HK13 = 2*Tbs(0,1);
+const float HK14 = (q1)*(q1);
+const float HK15 = (q2)*(q2);
+const float HK16 = -HK15;
+const float HK17 = (q0)*(q0);
+const float HK18 = (q3)*(q3);
+const float HK19 = HK17 - HK18;
+const float HK20 = q0*q2;
+const float HK21 = q1*q3;
+const float HK22 = 2*Tbs(0,2);
+const float HK23 = HK22*(HK20 + HK21) + Tbs(0,0)*(HK14 + HK16 + HK19);
+const float HK24 = q0*q1;
+const float HK25 = q2*q3;
+const float HK26 = HK24 - HK25;
+const float HK27 = -HK14;
+const float HK28 = 2*Tbs(0,0);
+const float HK29 = HK28*(HK10 + HK11) + Tbs(0,1)*(HK15 + HK19 + HK27);
+const float HK30 = HK20 - HK21;
+const float HK31 = HK13*(HK24 + HK25) + Tbs(0,2)*(HK16 + HK17 + HK18 + HK27);
+const float HK32 = 2*HK3;
+const float HK33 = -2*HK0*ve + 2*HK7;
+const float HK34 = -2*HK2*vd + 2*HK8;
+const float HK35 = -2*HK1*vn + 2*HK9;
+const float HK36 = -HK12*HK13 + HK23;
+const float HK37 = -HK22*HK26 + HK29;
+const float HK38 = -HK28*HK30 + HK31;
+const float HK39 = HK32*P(0,0) + HK33*P(0,1) + HK34*P(0,2) + HK35*P(0,3) + HK36*P(0,4) + HK37*P(0,5) + HK38*P(0,6);
+const float HK40 = 1.0F/((range)*(range));
+const float HK41 = HK32*P(0,6) + HK33*P(1,6) + HK34*P(2,6) + HK35*P(3,6) + HK36*P(4,6) + HK37*P(5,6) + HK38*P(6,6);
+const float HK42 = HK32*P(0,5) + HK33*P(1,5) + HK34*P(2,5) + HK35*P(3,5) + HK36*P(4,5) + HK37*P(5,5) + HK38*P(5,6);
+const float HK43 = HK32*P(0,4) + HK33*P(1,4) + HK34*P(2,4) + HK35*P(3,4) + HK36*P(4,4) + HK37*P(4,5) + HK38*P(4,6);
+const float HK44 = HK32*P(0,2) + HK33*P(1,2) + HK34*P(2,2) + HK35*P(2,3) + HK36*P(2,4) + HK37*P(2,5) + HK38*P(2,6);
+const float HK45 = HK32*P(0,3) + HK33*P(1,3) + HK34*P(2,3) + HK35*P(3,3) + HK36*P(3,4) + HK37*P(3,5) + HK38*P(3,6);
+const float HK46 = HK32*P(0,1) + HK33*P(1,1) + HK34*P(1,2) + HK35*P(1,3) + HK36*P(1,4) + HK37*P(1,5) + HK38*P(1,6);
+const float HK47 = HK4/(HK32*HK39*HK40 + HK33*HK40*HK46 + HK34*HK40*HK44 + HK35*HK40*HK45 + HK36*HK40*HK43 + HK37*HK40*HK42 + HK38*HK40*HK41 + R_LOS);
+
+
+// Observation Jacobians
+Hfusion.at<0>() = -HK3*HK5;
+Hfusion.at<1>() = -HK5*(-HK0*ve + HK7);
+Hfusion.at<2>() = -HK5*(-HK2*vd + HK8);
+Hfusion.at<3>() = -HK5*(-HK1*vn + HK9);
+Hfusion.at<4>() = -HK4*(-HK12*HK13 + HK23);
+Hfusion.at<5>() = -HK4*(-HK22*HK26 + HK29);
+Hfusion.at<6>() = -HK4*(-HK28*HK30 + HK31);
+
+
+// Kalman gains
+Kfusion(0) = -HK39*HK47;
+Kfusion(1) = -HK46*HK47;
+Kfusion(2) = -HK44*HK47;
+Kfusion(3) = -HK45*HK47;
+Kfusion(4) = -HK43*HK47;
+Kfusion(5) = -HK42*HK47;
+Kfusion(6) = -HK41*HK47;
+Kfusion(7) = -HK47*(HK32*P(0,7) + HK33*P(1,7) + HK34*P(2,7) + HK35*P(3,7) + HK36*P(4,7) + HK37*P(5,7) + HK38*P(6,7));
+Kfusion(8) = -HK47*(HK32*P(0,8) + HK33*P(1,8) + HK34*P(2,8) + HK35*P(3,8) + HK36*P(4,8) + HK37*P(5,8) + HK38*P(6,8));
+Kfusion(9) = -HK47*(HK32*P(0,9) + HK33*P(1,9) + HK34*P(2,9) + HK35*P(3,9) + HK36*P(4,9) + HK37*P(5,9) + HK38*P(6,9));
+Kfusion(10) = -HK47*(HK32*P(0,10) + HK33*P(1,10) + HK34*P(2,10) + HK35*P(3,10) + HK36*P(4,10) + HK37*P(5,10) + HK38*P(6,10));
+Kfusion(11) = -HK47*(HK32*P(0,11) + HK33*P(1,11) + HK34*P(2,11) + HK35*P(3,11) + HK36*P(4,11) + HK37*P(5,11) + HK38*P(6,11));
+Kfusion(12) = -HK47*(HK32*P(0,12) + HK33*P(1,12) + HK34*P(2,12) + HK35*P(3,12) + HK36*P(4,12) + HK37*P(5,12) + HK38*P(6,12));
+Kfusion(13) = -HK47*(HK32*P(0,13) + HK33*P(1,13) + HK34*P(2,13) + HK35*P(3,13) + HK36*P(4,13) + HK37*P(5,13) + HK38*P(6,13));
+Kfusion(14) = -HK47*(HK32*P(0,14) + HK33*P(1,14) + HK34*P(2,14) + HK35*P(3,14) + HK36*P(4,14) + HK37*P(5,14) + HK38*P(6,14));
+Kfusion(15) = -HK47*(HK32*P(0,15) + HK33*P(1,15) + HK34*P(2,15) + HK35*P(3,15) + HK36*P(4,15) + HK37*P(5,15) + HK38*P(6,15));
+Kfusion(16) = -HK47*(HK32*P(0,16) + HK33*P(1,16) + HK34*P(2,16) + HK35*P(3,16) + HK36*P(4,16) + HK37*P(5,16) + HK38*P(6,16));
+Kfusion(17) = -HK47*(HK32*P(0,17) + HK33*P(1,17) + HK34*P(2,17) + HK35*P(3,17) + HK36*P(4,17) + HK37*P(5,17) + HK38*P(6,17));
+Kfusion(18) = -HK47*(HK32*P(0,18) + HK33*P(1,18) + HK34*P(2,18) + HK35*P(3,18) + HK36*P(4,18) + HK37*P(5,18) + HK38*P(6,18));
+Kfusion(19) = -HK47*(HK32*P(0,19) + HK33*P(1,19) + HK34*P(2,19) + HK35*P(3,19) + HK36*P(4,19) + HK37*P(5,19) + HK38*P(6,19));
+Kfusion(20) = -HK47*(HK32*P(0,20) + HK33*P(1,20) + HK34*P(2,20) + HK35*P(3,20) + HK36*P(4,20) + HK37*P(5,20) + HK38*P(6,20));
+Kfusion(21) = -HK47*(HK32*P(0,21) + HK33*P(1,21) + HK34*P(2,21) + HK35*P(3,21) + HK36*P(4,21) + HK37*P(5,21) + HK38*P(6,21));
+Kfusion(22) = -HK47*(HK32*P(0,22) + HK33*P(1,22) + HK34*P(2,22) + HK35*P(3,22) + HK36*P(4,22) + HK37*P(5,22) + HK38*P(6,22));
+Kfusion(23) = -HK47*(HK32*P(0,23) + HK33*P(1,23) + HK34*P(2,23) + HK35*P(3,23) + HK36*P(4,23) + HK37*P(5,23) + HK38*P(6,23));
+
+
+// Predicted observation
+
+
+// Innovation variance
+
+

src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated_alt.cpp

@@ -0,0 +1,157 @@
+// Sub Expressions
+const float HK0 = -Tbs(1,0)*q2 + Tbs(1,1)*q1 + Tbs(1,2)*q0;
+const float HK1 = Tbs(1,0)*q3 + Tbs(1,1)*q0 - Tbs(1,2)*q1;
+const float HK2 = Tbs(1,0)*q0 - Tbs(1,1)*q3 + Tbs(1,2)*q2;
+const float HK3 = HK0*vd + HK1*ve + HK2*vn;
+const float HK4 = 1.0F/(range);
+const float HK5 = 2*HK4;
+const float HK6 = Tbs(1,0)*q1 + Tbs(1,1)*q2 + Tbs(1,2)*q3;
+const float HK7 = -HK0*ve + HK1*vd + HK6*vn;
+const float HK8 = HK0*vn - HK2*vd + HK6*ve;
+const float HK9 = -HK1*vn + HK2*ve + HK6*vd;
+const float HK10 = q0*q2;
+const float HK11 = q1*q3;
+const float HK12 = HK10 + HK11;
+const float HK13 = 2*Tbs(1,2);
+const float HK14 = q0*q3;
+const float HK15 = q1*q2;
+const float HK16 = HK14 - HK15;
+const float HK17 = 2*Tbs(1,1);
+const float HK18 = (q1)*(q1);
+const float HK19 = (q2)*(q2);
+const float HK20 = -HK19;
+const float HK21 = (q0)*(q0);
+const float HK22 = (q3)*(q3);
+const float HK23 = HK21 - HK22;
+const float HK24 = HK18 + HK20 + HK23;
+const float HK25 = HK12*HK13 - HK16*HK17 + HK24*Tbs(1,0);
+const float HK26 = HK14 + HK15;
+const float HK27 = 2*Tbs(1,0);
+const float HK28 = q0*q1;
+const float HK29 = q2*q3;
+const float HK30 = HK28 - HK29;
+const float HK31 = -HK18;
+const float HK32 = HK19 + HK23 + HK31;
+const float HK33 = -HK13*HK30 + HK26*HK27 + HK32*Tbs(1,1);
+const float HK34 = HK28 + HK29;
+const float HK35 = HK10 - HK11;
+const float HK36 = HK20 + HK21 + HK22 + HK31;
+const float HK37 = HK17*HK34 - HK27*HK35 + HK36*Tbs(1,2);
+const float HK38 = 2*HK3;
+const float HK39 = 2*HK7;
+const float HK40 = 2*HK8;
+const float HK41 = 2*HK9;
+const float HK42 = HK25*P(0,4) + HK33*P(0,5) + HK37*P(0,6) + HK38*P(0,0) + HK39*P(0,1) + HK40*P(0,2) + HK41*P(0,3);
+const float HK43 = 1.0F/((range)*(range));
+const float HK44 = HK25*P(4,6) + HK33*P(5,6) + HK37*P(6,6) + HK38*P(0,6) + HK39*P(1,6) + HK40*P(2,6) + HK41*P(3,6);
+const float HK45 = HK25*P(4,5) + HK33*P(5,5) + HK37*P(5,6) + HK38*P(0,5) + HK39*P(1,5) + HK40*P(2,5) + HK41*P(3,5);
+const float HK46 = HK25*P(4,4) + HK33*P(4,5) + HK37*P(4,6) + HK38*P(0,4) + HK39*P(1,4) + HK40*P(2,4) + HK41*P(3,4);
+const float HK47 = HK25*P(2,4) + HK33*P(2,5) + HK37*P(2,6) + HK38*P(0,2) + HK39*P(1,2) + HK40*P(2,2) + HK41*P(2,3);
+const float HK48 = HK25*P(3,4) + HK33*P(3,5) + HK37*P(3,6) + HK38*P(0,3) + HK39*P(1,3) + HK40*P(2,3) + HK41*P(3,3);
+const float HK49 = HK25*P(1,4) + HK33*P(1,5) + HK37*P(1,6) + HK38*P(0,1) + HK39*P(1,1) + HK40*P(1,2) + HK41*P(1,3);
+const float HK50 = HK4/(HK25*HK43*HK46 + HK33*HK43*HK45 + HK37*HK43*HK44 + HK38*HK42*HK43 + HK39*HK43*HK49 + HK40*HK43*HK47 + HK41*HK43*HK48 + R_LOS);
+const float HK51 = -Tbs(0,0)*q2 + Tbs(0,1)*q1 + Tbs(0,2)*q0;
+const float HK52 = Tbs(0,0)*q3 + Tbs(0,1)*q0 - Tbs(0,2)*q1;
+const float HK53 = Tbs(0,0)*q0 - Tbs(0,1)*q3 + Tbs(0,2)*q2;
+const float HK54 = HK51*vd + HK52*ve + HK53*vn;
+const float HK55 = Tbs(0,0)*q1 + Tbs(0,1)*q2 + Tbs(0,2)*q3;
+const float HK56 = HK52*vd + HK55*vn;
+const float HK57 = HK51*vn + HK55*ve;
+const float HK58 = HK53*ve + HK55*vd;
+const float HK59 = 2*Tbs(0,1);
+const float HK60 = 2*Tbs(0,2);
+const float HK61 = HK12*HK60 + HK24*Tbs(0,0);
+const float HK62 = 2*Tbs(0,0);
+const float HK63 = HK26*HK62 + HK32*Tbs(0,1);
+const float HK64 = HK34*HK59 + HK36*Tbs(0,2);
+const float HK65 = 2*HK54;
+const float HK66 = -2*HK51*ve + 2*HK56;
+const float HK67 = -2*HK53*vd + 2*HK57;
+const float HK68 = -2*HK52*vn + 2*HK58;
+const float HK69 = -HK16*HK59 + HK61;
+const float HK70 = -HK30*HK60 + HK63;
+const float HK71 = -HK35*HK62 + HK64;
+const float HK72 = HK65*P(0,0) + HK66*P(0,1) + HK67*P(0,2) + HK68*P(0,3) + HK69*P(0,4) + HK70*P(0,5) + HK71*P(0,6);
+const float HK73 = HK65*P(0,6) + HK66*P(1,6) + HK67*P(2,6) + HK68*P(3,6) + HK69*P(4,6) + HK70*P(5,6) + HK71*P(6,6);
+const float HK74 = HK65*P(0,5) + HK66*P(1,5) + HK67*P(2,5) + HK68*P(3,5) + HK69*P(4,5) + HK70*P(5,5) + HK71*P(5,6);
+const float HK75 = HK65*P(0,4) + HK66*P(1,4) + HK67*P(2,4) + HK68*P(3,4) + HK69*P(4,4) + HK70*P(4,5) + HK71*P(4,6);
+const float HK76 = HK65*P(0,2) + HK66*P(1,2) + HK67*P(2,2) + HK68*P(2,3) + HK69*P(2,4) + HK70*P(2,5) + HK71*P(2,6);
+const float HK77 = HK65*P(0,3) + HK66*P(1,3) + HK67*P(2,3) + HK68*P(3,3) + HK69*P(3,4) + HK70*P(3,5) + HK71*P(3,6);
+const float HK78 = HK65*P(0,1) + HK66*P(1,1) + HK67*P(1,2) + HK68*P(1,3) + HK69*P(1,4) + HK70*P(1,5) + HK71*P(1,6);
+const float HK79 = HK4/(HK43*HK65*HK72 + HK43*HK66*HK78 + HK43*HK67*HK76 + HK43*HK68*HK77 + HK43*HK69*HK75 + HK43*HK70*HK74 + HK43*HK71*HK73 + R_LOS);
+
+
+// Observation Jacobians - axis 0
+Hfusion.at<0>() = HK3*HK5;
+Hfusion.at<1>() = HK5*HK7;
+Hfusion.at<2>() = HK5*HK8;
+Hfusion.at<3>() = HK5*HK9;
+Hfusion.at<4>() = HK25*HK4;
+Hfusion.at<5>() = HK33*HK4;
+Hfusion.at<6>() = HK37*HK4;
+
+
+// Kalman gains - axis 0
+Kfusion(0) = HK42*HK50;
+Kfusion(1) = HK49*HK50;
+Kfusion(2) = HK47*HK50;
+Kfusion(3) = HK48*HK50;
+Kfusion(4) = HK46*HK50;
+Kfusion(5) = HK45*HK50;
+Kfusion(6) = HK44*HK50;
+Kfusion(7) = HK50*(HK25*P(4,7) + HK33*P(5,7) + HK37*P(6,7) + HK38*P(0,7) + HK39*P(1,7) + HK40*P(2,7) + HK41*P(3,7));
+Kfusion(8) = HK50*(HK25*P(4,8) + HK33*P(5,8) + HK37*P(6,8) + HK38*P(0,8) + HK39*P(1,8) + HK40*P(2,8) + HK41*P(3,8));
+Kfusion(9) = HK50*(HK25*P(4,9) + HK33*P(5,9) + HK37*P(6,9) + HK38*P(0,9) + HK39*P(1,9) + HK40*P(2,9) + HK41*P(3,9));
+Kfusion(10) = HK50*(HK25*P(4,10) + HK33*P(5,10) + HK37*P(6,10) + HK38*P(0,10) + HK39*P(1,10) + HK40*P(2,10) + HK41*P(3,10));
+Kfusion(11) = HK50*(HK25*P(4,11) + HK33*P(5,11) + HK37*P(6,11) + HK38*P(0,11) + HK39*P(1,11) + HK40*P(2,11) + HK41*P(3,11));
+Kfusion(12) = HK50*(HK25*P(4,12) + HK33*P(5,12) + HK37*P(6,12) + HK38*P(0,12) + HK39*P(1,12) + HK40*P(2,12) + HK41*P(3,12));
+Kfusion(13) = HK50*(HK25*P(4,13) + HK33*P(5,13) + HK37*P(6,13) + HK38*P(0,13) + HK39*P(1,13) + HK40*P(2,13) + HK41*P(3,13));
+Kfusion(14) = HK50*(HK25*P(4,14) + HK33*P(5,14) + HK37*P(6,14) + HK38*P(0,14) + HK39*P(1,14) + HK40*P(2,14) + HK41*P(3,14));
+Kfusion(15) = HK50*(HK25*P(4,15) + HK33*P(5,15) + HK37*P(6,15) + HK38*P(0,15) + HK39*P(1,15) + HK40*P(2,15) + HK41*P(3,15));
+Kfusion(16) = HK50*(HK25*P(4,16) + HK33*P(5,16) + HK37*P(6,16) + HK38*P(0,16) + HK39*P(1,16) + HK40*P(2,16) + HK41*P(3,16));
+Kfusion(17) = HK50*(HK25*P(4,17) + HK33*P(5,17) + HK37*P(6,17) + HK38*P(0,17) + HK39*P(1,17) + HK40*P(2,17) + HK41*P(3,17));
+Kfusion(18) = HK50*(HK25*P(4,18) + HK33*P(5,18) + HK37*P(6,18) + HK38*P(0,18) + HK39*P(1,18) + HK40*P(2,18) + HK41*P(3,18));
+Kfusion(19) = HK50*(HK25*P(4,19) + HK33*P(5,19) + HK37*P(6,19) + HK38*P(0,19) + HK39*P(1,19) + HK40*P(2,19) + HK41*P(3,19));
+Kfusion(20) = HK50*(HK25*P(4,20) + HK33*P(5,20) + HK37*P(6,20) + HK38*P(0,20) + HK39*P(1,20) + HK40*P(2,20) + HK41*P(3,20));
+Kfusion(21) = HK50*(HK25*P(4,21) + HK33*P(5,21) + HK37*P(6,21) + HK38*P(0,21) + HK39*P(1,21) + HK40*P(2,21) + HK41*P(3,21));
+Kfusion(22) = HK50*(HK25*P(4,22) + HK33*P(5,22) + HK37*P(6,22) + HK38*P(0,22) + HK39*P(1,22) + HK40*P(2,22) + HK41*P(3,22));
+Kfusion(23) = HK50*(HK25*P(4,23) + HK33*P(5,23) + HK37*P(6,23) + HK38*P(0,23) + HK39*P(1,23) + HK40*P(2,23) + HK41*P(3,23));
+
+
+// Observation Jacobians - axis 1
+Hfusion.at<0>() = -HK5*HK54;
+Hfusion.at<1>() = -HK5*(-HK51*ve + HK56);
+Hfusion.at<2>() = -HK5*(-HK53*vd + HK57);
+Hfusion.at<3>() = -HK5*(-HK52*vn + HK58);
+Hfusion.at<4>() = -HK4*(-HK16*HK59 + HK61);
+Hfusion.at<5>() = -HK4*(-HK30*HK60 + HK63);
+Hfusion.at<6>() = -HK4*(-HK35*HK62 + HK64);
+
+
+// Kalman gains - axis 1
+Kfusion(0) = -HK72*HK79;
+Kfusion(1) = -HK78*HK79;
+Kfusion(2) = -HK76*HK79;
+Kfusion(3) = -HK77*HK79;
+Kfusion(4) = -HK75*HK79;
+Kfusion(5) = -HK74*HK79;
+Kfusion(6) = -HK73*HK79;
+Kfusion(7) = -HK79*(HK65*P(0,7) + HK66*P(1,7) + HK67*P(2,7) + HK68*P(3,7) + HK69*P(4,7) + HK70*P(5,7) + HK71*P(6,7));
+Kfusion(8) = -HK79*(HK65*P(0,8) + HK66*P(1,8) + HK67*P(2,8) + HK68*P(3,8) + HK69*P(4,8) + HK70*P(5,8) + HK71*P(6,8));
+Kfusion(9) = -HK79*(HK65*P(0,9) + HK66*P(1,9) + HK67*P(2,9) + HK68*P(3,9) + HK69*P(4,9) + HK70*P(5,9) + HK71*P(6,9));
+Kfusion(10) = -HK79*(HK65*P(0,10) + HK66*P(1,10) + HK67*P(2,10) + HK68*P(3,10) + HK69*P(4,10) + HK70*P(5,10) + HK71*P(6,10));
+Kfusion(11) = -HK79*(HK65*P(0,11) + HK66*P(1,11) + HK67*P(2,11) + HK68*P(3,11) + HK69*P(4,11) + HK70*P(5,11) + HK71*P(6,11));
+Kfusion(12) = -HK79*(HK65*P(0,12) + HK66*P(1,12) + HK67*P(2,12) + HK68*P(3,12) + HK69*P(4,12) + HK70*P(5,12) + HK71*P(6,12));
+Kfusion(13) = -HK79*(HK65*P(0,13) + HK66*P(1,13) + HK67*P(2,13) + HK68*P(3,13) + HK69*P(4,13) + HK70*P(5,13) + HK71*P(6,13));
+Kfusion(14) = -HK79*(HK65*P(0,14) + HK66*P(1,14) + HK67*P(2,14) + HK68*P(3,14) + HK69*P(4,14) + HK70*P(5,14) + HK71*P(6,14));
+Kfusion(15) = -HK79*(HK65*P(0,15) + HK66*P(1,15) + HK67*P(2,15) + HK68*P(3,15) + HK69*P(4,15) + HK70*P(5,15) + HK71*P(6,15));
+Kfusion(16) = -HK79*(HK65*P(0,16) + HK66*P(1,16) + HK67*P(2,16) + HK68*P(3,16) + HK69*P(4,16) + HK70*P(5,16) + HK71*P(6,16));
+Kfusion(17) = -HK79*(HK65*P(0,17) + HK66*P(1,17) + HK67*P(2,17) + HK68*P(3,17) + HK69*P(4,17) + HK70*P(5,17) + HK71*P(6,17));
+Kfusion(18) = -HK79*(HK65*P(0,18) + HK66*P(1,18) + HK67*P(2,18) + HK68*P(3,18) + HK69*P(4,18) + HK70*P(5,18) + HK71*P(6,18));
+Kfusion(19) = -HK79*(HK65*P(0,19) + HK66*P(1,19) + HK67*P(2,19) + HK68*P(3,19) + HK69*P(4,19) + HK70*P(5,19) + HK71*P(6,19));
+Kfusion(20) = -HK79*(HK65*P(0,20) + HK66*P(1,20) + HK67*P(2,20) + HK68*P(3,20) + HK69*P(4,20) + HK70*P(5,20) + HK71*P(6,20));
+Kfusion(21) = -HK79*(HK65*P(0,21) + HK66*P(1,21) + HK67*P(2,21) + HK68*P(3,21) + HK69*P(4,21) + HK70*P(5,21) + HK71*P(6,21));
+Kfusion(22) = -HK79*(HK65*P(0,22) + HK66*P(1,22) + HK67*P(2,22) + HK68*P(3,22) + HK69*P(4,22) + HK70*P(5,22) + HK71*P(6,22));
+Kfusion(23) = -HK79*(HK65*P(0,23) + HK66*P(1,23) + HK67*P(2,23) + HK68*P(3,23) + HK69*P(4,23) + HK70*P(5,23) + HK71*P(6,23));
+
+

src/modules/ekf2/EKF/terrain_estimator.cpp

@@ -45,14 +45,7 @@
 
 void Ekf::initHagl()
 {
-	stopHaglFlowFusion();
-	stopHaglRngFusion();
-
-	// assume a ground clearance
-	_terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance;
-
-	// use the ground clearance value as our uncertainty
-	_terrain_var = sq(_params.rng_gnd_clearance);
+	resetHaglFake();
 }
 
 void Ekf::runTerrainEstimator()
@@ -73,6 +66,8 @@ void Ekf::runTerrainEstimator()
 	if (_terrain_vpos - _state.pos(2) < _params.rng_gnd_clearance) {
 		_terrain_vpos = _params.rng_gnd_clearance + _state.pos(2);
 	}
+
+	updateTerrainValidity();
 }
 
 void Ekf::predictHagl()
@@ -311,7 +306,7 @@ void Ekf::stopHaglFlowFusion()
 void Ekf::resetHaglFlow()
 {
 	// TODO: use the flow data
-	_terrain_vpos = fmaxf(0.0f, _state.pos(2));
+	_terrain_vpos = fmaxf(0.0f,  _state.pos(2));
 	_terrain_var = 100.0f;
 	_terrain_vpos_reset_counter++;
 }
@@ -415,23 +410,27 @@ void Ekf::controlHaglFakeFusion()
 	if (!_control_status.flags.in_air
 	    && !_hagl_sensor_status.flags.range_finder
 	    && !_hagl_sensor_status.flags.flow) {
-
-		initHagl();
+		resetHaglFake();
 	}
 }
 
-bool Ekf::isTerrainEstimateValid() const
+void Ekf::resetHaglFake()
+{
+	// assume a ground clearance
+	_terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance;
+	// use the ground clearance value as our uncertainty
+	_terrain_var = sq(_params.rng_gnd_clearance);
+	_time_last_hagl_fuse = _imu_sample_delayed.time_us;
+}
+
+void Ekf::updateTerrainValidity()
 {
 	// we have been fusing range finder measurements in the last 5 seconds
-	if (_hagl_sensor_status.flags.range_finder && isRecent(_time_last_hagl_fuse, (uint64_t)5e6)) {
-		return true;
-	}
+	const bool recent_range_fusion = isRecent(_time_last_hagl_fuse, (uint64_t)5e6);
 
 	// we have been fusing optical flow measurements for terrain estimation within the last 5 seconds
 	// this can only be the case if the main filter does not fuse optical flow
-	if (_hagl_sensor_status.flags.flow && isRecent(_time_last_flow_terrain_fuse, (uint64_t)5e6)) {
-		return true;
-	}
+	const bool recent_flow_for_terrain_fusion = isRecent(_time_last_flow_terrain_fuse, (uint64_t)5e6);
 
-	return false;
+	_hagl_valid = (recent_range_fusion || recent_flow_for_terrain_fusion);
 }

src/modules/ekf2/EKF/zero_velocity_update.cpp

@@ -45,16 +45,15 @@ void Ekf::controlZeroVelocityUpdate()
 
 	if (zero_velocity_update_data_ready) {
 		const bool continuing_conditions_passing = _control_status.flags.vehicle_at_rest
-				&& _control_status_prev.flags.vehicle_at_rest
-				&& !isVerticalVelocityAidingActive(); // otherwise the filter is "too rigid" to follow a position drift
+				&& _control_status_prev.flags.vehicle_at_rest;
 
 		if (continuing_conditions_passing) {
 			Vector3f vel_obs{0, 0, 0};
 			Vector3f innovation = _state.vel - vel_obs;
 
-			// Set a low variance initially for faster leveling and higher
+			// Set a low variance initially for faster accel bias learning and higher
 			// later to let the states follow the measurements
-			const float obs_var = _control_status.flags.tilt_align ? sq(0.2f) : sq(0.001f);
+			const float obs_var = _NED_origin_initialised ? sq(0.2f) : sq(0.001f);
 			Vector3f innov_var{
 				P(4, 4) + obs_var,
 				P(5, 5) + obs_var,