mavlink: remove default device and consolidate serial vs UDP init handling

Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>

.ci/Jenkinsfile-hardware

@@ -920,7 +920,6 @@ 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,17 +1,29 @@
-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.
+<!--
 
-## 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...
+Thank you for your contribution!
 
-## Describe your solution
-A clear and concise description of what you have implemented.
+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 possible alternatives
-A clear and concise description of alternative solutions or features you've considered.
+-->
 
-## 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.
+### Solved Problem
+When ... I found that ...
 
-## Additional context
-Add any other related context or media.
+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

Kconfig

@@ -190,3 +190,7 @@ 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,6 +10,7 @@
 # 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,23 +264,6 @@ 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
 	#
@@ -527,6 +510,23 @@ 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.GetName()
+            result += '### %s\n\n' % group.GetType()
 
             # 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.GetParams())
-            for param in group.GetParams():
-                if not self.IsExcluded(param, board):
-                    for output_name in param.GetOutputCodes():
-                        value = param.GetOutputValue(output_name)
+            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)
                         key_value_pair = (output_name, value)
                         if key_value_pair not in all_outputs:
                             all_outputs[key_value_pair] = 0
@@ -104,18 +104,17 @@ div.frame_variant td, div.frame_variant th {
             result += ' </thead>\n'
             result += '<tbody>\n'
 
-            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()
+            for airframe in group.GetAirframes():
+                if not self.IsExcluded(airframe, board):
+                    name = airframe.GetName()
+                    airframe_id = airframe.GetId()
                     airframe_id_entry = '<p><code>SYS_AUTOSTART</code> = %s</p>' % (airframe_id)
-                    maintainer = param.GetMaintainer()
+                    maintainer = airframe.GetMaintainer()
                     maintainer_entry = ''
                     if maintainer != '':
                         maintainer_entry = 'Maintainer: %s' % (html.escape(maintainer))
-                    url = param.GetFieldValue('url')
-                    name_anchor='%s_%s_%s' % (group.GetClass(),group.GetName(),name)
+                    url = airframe.GetFieldValue('url')
+                    name_anchor='%s_%s_%s' % (group.GetClass(),group.GetType(),name)
                     name_anchor=name_anchor.replace(' ','_').lower()
                     name_anchor=name_anchor.replace('"','_').lower()
                     name_anchor='id="%s"' % name_anchor
@@ -124,8 +123,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 param.GetOutputCodes():
-                        value = param.GetOutputValue(output_name)
+                    for output_name in airframe.GetOutputCodes():
+                        value = airframe.GetOutputValue(output_name)
                         valstrs = value.split(";")
                         key_value_pair = (output_name, value)
                         if all_outputs[key_value_pair] < num_configs:
@@ -152,9 +151,9 @@ div.frame_variant td, div.frame_variant th {
 
         self.output = result
 
-    def IsExcluded(self, param, board):
-        for code in param.GetArchCodes():
-            if "CONFIG_ARCH_BOARD_{0}".format(code) == board and param.GetArchValue(code) == "exclude":
+    def IsExcluded(self, airframe, board):
+        for code in airframe.GetArchCodes():
+            if "CONFIG_ARCH_BOARD_{0}".format(code) == board and airframe.GetArchValue(code) == "exclude":
                 return True
         return False
 

Tools/px4airframes/rcout.py

@@ -3,6 +3,9 @@ import codecs
 import os
 
 class RCOutput():
+    """
+        Generates RC scripts for the airframes
+    """
     def __init__(self, groups, board, post_start=False):
 
         result = (  "#\n"
@@ -34,33 +37,33 @@ class RCOutput():
         result += "set AIRFRAME none\n"
         result += "\n"
         for group in groups:
-            result += "# GROUP: %s\n\n" % group.GetName()
-            for param in group.GetParams():
+            result += "# GROUP: %s\n\n" % group.GetType()
+            for airframe in group.GetAirframes():
                 excluded = False
-                for code in param.GetArchCodes():
-                    if "{0}".format(code) == board and param.GetArchValue(code) == "exclude":
+                for code in airframe.GetArchCodes():
+                    if "{0}".format(code) == board and airframe.GetArchValue(code) == "exclude":
                         excluded = True
                 if excluded:
                     continue
 
                 if post_start:
                     # Path to post-start sript
-                    path = param.GetPostPath()
+                    path = airframe.GetPostPath()
                 else:
                     # Path to start script
-                    path = param.GetPath()
+                    path = airframe.GetPath()
 
                 if not path:
                     continue
 
                 path = os.path.split(path)[1]
 
-                id_val = param.GetId()
-                name = param.GetFieldValue("short_desc")
-                long_desc = param.GetFieldValue("long_desc")
+                id_val = airframe.GetId()
+                name = airframe.GetFieldValue("short_desc")
+                long_desc = airframe.GetFieldValue("long_desc")
 
                 result +=   "#\n"
-                result +=   "# %s\n" % param.GetName()
+                result +=   "# %s\n" % airframe.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,31 +2,38 @@ import sys
 import re
 import os
 
-class ParameterGroup(object):
+class AirframeGroup(object):
     """
-    Single parameter group
+    Airframe group
+
+    type: specific vehicle type (e.g. VTOL Tiltrotor, VTOL Quadrotor, etc.)
+    class: vehicle class (e.g. Multicopter, Fixed Wing, etc.)
     """
-    def __init__(self, name, af_class):
-        self.name = name
+    def __init__(self, type, af_class):
+        self.type = type
         self.af_class = af_class
-        self.params = []
+        self.airframes = []
 
 
-    def AddParameter(self, param):
+    def AddAirframe(self, airframe):
         """
-        Add parameter to the group
+        Add airframe to the airframe group
         """
-        self.params.append(param)
+        self.airframes.append(airframe)
 
-    def GetName(self):
+    def GetType(self):
         """
-        Get parameter group name
+        Get airframe group's vehicle type
+
+        e.g. VTOL Tiltrotor, VTOL Quadrotor, etc.
         """
-        return self.name
+        return self.type
 
     def GetClass(self):
         """
-        Get parameter group vehicle type.
+        Get airframe group's vehicle class
+
+        e.g. Multicopter, Fixed Wing, etc.
         """
         return self.af_class
 
@@ -34,86 +41,84 @@ class ParameterGroup(object):
         """
         Get parameter group image base name (w/o extension)
         """
-        if (self.name == "Standard Plane"):
+        if (self.type == "Standard Plane"):
             return "Plane"
-        elif (self.name == "Flying Wing"):
+        elif (self.type == "Flying Wing"):
             return "FlyingWing"
-        elif (self.name == "Quadrotor x"):
+        elif (self.type == "Quadrotor x"):
             return "QuadRotorX"
-        elif (self.name == "Quadrotor +"):
+        elif (self.type == "Quadrotor +"):
             return "QuadRotorPlus"
-        elif (self.name == "Hexarotor x"):
+        elif (self.type == "Hexarotor x"):
             return "HexaRotorX"
-        elif (self.name == "Hexarotor +"):
+        elif (self.type == "Hexarotor +"):
             return "HexaRotorPlus"
-        elif (self.name == "Octorotor +"):
+        elif (self.type == "Octorotor +"):
             return "OctoRotorPlus"
-        elif (self.name == "Octorotor x"):
+        elif (self.type == "Octorotor x"):
             return "OctoRotorX"
-        elif (self.name == "Octorotor Coaxial"):
+        elif (self.type == "Octorotor Coaxial"):
             return "OctoRotorXCoaxial"
-        elif (self.name == "Octo Coax Wide"):
+        elif (self.type == "Octo Coax Wide"):
             return "OctoRotorXCoaxial"
-        elif (self.name == "Quadrotor Wide"):
+        elif (self.type == "Quadrotor Wide"):
             return "QuadRotorWide"
-        elif (self.name == "Quadrotor H"):
+        elif (self.type == "Quadrotor H"):
             return "QuadRotorH"
-        elif (self.name == "Dodecarotor cox"):
+        elif (self.type == "Dodecarotor cox"):
             return "DodecaRotorXCoaxial"
-        elif (self.name == "Simulation"):
+        elif (self.type == "Simulation"):
             return "AirframeSimulation"
-        elif (self.name == "Plane A-Tail"):
+        elif (self.type == "Plane A-Tail"):
             return "PlaneATail"
-        elif (self.name == "Plane V-Tail"):
+        elif (self.type == "Plane V-Tail"):
             return "PlaneVTail"
-        elif (self.name == "VTOL Duo Tailsitter"):
+        elif (self.type == "VTOL Duo Tailsitter"):
             return "VTOLDuoRotorTailSitter"
-        elif (self.name == "Standard VTOL"):
+        elif (self.type == "Standard VTOL"):
             return "VTOLPlane"
-        elif (self.name == "VTOL Quad Tailsitter"):
+        elif (self.type == "VTOL Quad Tailsitter"):
             return "VTOLQuadRotorTailSitter"
-        elif (self.name == "VTOL Tiltrotor"):
+        elif (self.type == "VTOL Tiltrotor"):
             return "VTOLTiltRotor"
-        elif (self.name == "VTOL Octoplane"):
+        elif (self.type == "VTOL Octoplane"):
             return "VTOLPlaneOcto"
-        elif (self.name == "Coaxial Helicopter"):
+        elif (self.type == "Coaxial Helicopter"):
             return "HelicopterCoaxial"
-        elif (self.name == "Helicopter"):
+        elif (self.type == "Helicopter"):
             return "Helicopter"
-        elif (self.name == "Hexarotor Coaxial"):
+        elif (self.type == "Hexarotor Coaxial"):
             return "Y6B"
-        elif (self.name == "Y6A"):
+        elif (self.type == "Y6A"):
             return "Y6A"
-        elif (self.name == "Tricopter Y-"):
+        elif (self.type == "Tricopter Y-"):
             return "YMinus"
-        elif (self.name == "Tricopter Y+"):
+        elif (self.type == "Tricopter Y+"):
             return "YPlus"
-        elif (self.name == "Autogyro"):
+        elif (self.type == "Autogyro"):
             return "Autogyro"
-        elif (self.name == "Airship"):
+        elif (self.type == "Airship"):
             return "Airship"
-        elif (self.name == "Rover"):
+        elif (self.type == "Rover"):
             return "Rover"
-        elif (self.name == "Boat"):
+        elif (self.type == "Boat"):
             return "Boat"
-        elif (self.name == "Balloon"):
+        elif (self.type == "Balloon"):
             return "Balloon"
-        elif (self.name == "Vectored 6 DOF UUV"):
+        elif (self.type == "Vectored 6 DOF UUV"):
             return "Vectored6DofUUV"
         return "AirframeUnknown"
 
-    def GetParams(self):
+    def GetAirframes(self):
         """
-        Returns the parsed list of parameters. Every parameter is a Parameter
-        object. Note that returned object is not a copy. Modifications affect
-        state of the parser.
+        Returns the parsed list of airframes objects. Note that returned
+        object is not a copy. Modifications affect state of the parser.
         """
+        return sorted(self.airframes, key=lambda x: x.GetId())
 
-        return sorted(self.params, key=lambda x: x.GetId())
-
-class Parameter(object):
+class Airframe(object):
     """
-    Single parameter
+    Single Airframe definition
     """
 
     # Define sorting order of the fields
@@ -288,7 +293,7 @@ class SourceParser(object):
     }
 
     def __init__(self):
-        self.param_groups = {}
+        self.airframe_groups = {}
 
     def GetSupportedExtensions(self):
         """
@@ -347,10 +352,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
@@ -427,7 +432,7 @@ class SourceParser(object):
             post_path = None
 
         # We already know this is an airframe config, so add it
-        param = Parameter(path, post_path, airframe_name, airframe_type, airframe_class, airframe_id, maintainer)
+        airframe = Airframe(path, post_path, airframe_name, airframe_type, airframe_class, airframe_id, maintainer)
 
         # Done with file, store
         for tag in tags:
@@ -440,24 +445,24 @@ class SourceParser(object):
             if tag == "name":
                 airframe_name = tags[tag]
             else:
-                param.SetField(tag, tags[tag])
+                airframe.SetField(tag, tags[tag])
 
         # Store outputs
         for output in outputs:
-            param.SetOutput(output, outputs[output])
+            airframe.SetOutput(output, outputs[output])
 
         # Store outputs
         for arch in archs:
-            param.SetArch(arch, archs[arch])
+            airframe.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.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)
+        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)
 
         return True
 
@@ -473,8 +478,8 @@ class SourceParser(object):
         Validates the airframe meta data.
         """
         seenParamNames = []
-        for group in self.GetParamGroups():
-            for param in group.GetParams():
+        for group in self.GetAirframeGroups():
+            for param in group.GetAirframes():
                 name  = param.GetName()
                 board = param.GetFieldValue("board")
                 # Check for duplicates
@@ -487,27 +492,27 @@ class SourceParser(object):
 
         return True
 
-    def GetParamGroups(self):
+    def GetAirframeGroups(self):
         """
-        Returns the parsed list of parameters. Every parameter is a Parameter
+        Returns the parsed list of Airframe groups. Every Airframe is an Airframe
         object. Note that returned object is not a copy. Modifications affect
         state of the parser.
         """
-        groups = self.param_groups.values()
-        groups = sorted(groups, key=lambda x: x.GetName())
+        groups = self.airframe_groups.values()
+        groups = sorted(groups, key=lambda x: x.GetType())
         groups = sorted(groups, key=lambda x: x.GetClass())
-        groups = sorted(groups, key=lambda x: self.priority.get(x.GetName(), 0), reverse=True)
+        groups = sorted(groups, key=lambda x: self.priority.get(x.GetType(), 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.GetName() in duplicate_test:
-                duplicate_set.add(group.GetName())
+            if group.GetType() in duplicate_test:
+                duplicate_set.add(group.GetType())
             else:
-                duplicate_test.add(group.GetName() )
+                duplicate_test.add(group.GetType() )
         for group in groups:
-            if group.GetName() in duplicate_set:
-                group.name=group.GetName()+' (%s)' % group.GetClass()
+            if group.GetType() in duplicate_set:
+                group.name=group.GetType()+' (%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.GetName()
+            xml_group.attrib["name"] = group.GetType()
             xml_group.attrib["image"] = group.GetImageName()
-            for param in group.GetParams():
+            for airframe in group.GetAirframes():
 
                 # check if there is an exclude tag for this airframe
                 excluded = False
-                for code in param.GetArchCodes():
-                    if "CONFIG_ARCH_BOARD_{0}".format(code) == board and param.GetArchValue(code) == "exclude":
+                for code in airframe.GetArchCodes():
+                    if "CONFIG_ARCH_BOARD_{0}".format(code) == board and airframe.GetArchValue(code) == "exclude":
                         excluded = True
 
                 if not excluded:
-                    #print("generating: {0} {1}".format(param.GetName(), excluded))
+                    #print("generating: {0} {1}".format(airframe.GetName(), excluded))
                     xml_param = ET.SubElement(xml_group, "airframe")
-                    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_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_field = ET.SubElement(xml_param, code)
                         xml_field.text = value
-                    for code in param.GetOutputCodes():
-                        value = param.GetOutputValue(code)
+                    for code in airframe.GetOutputCodes():
+                        value = airframe.GetOutputValue(code)
                         valstrs = value.split(";")
                         xml_field = ET.SubElement(xml_param, "output")
                         xml_field.attrib["name"] = code

Tools/px_process_airframes.py

@@ -35,12 +35,11 @@
 #
 # PX4 airframe config processor (main executable file)
 #
-# 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)
+# This tool scans the PX4 ROMFS directory for declarations of airframes
 #
+# 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
@@ -104,31 +103,31 @@ def main():
     # We can't validate yet
     # if not parser.Validate():
     #     sys.exit(1)
-    param_groups = parser.GetParamGroups()
+    airframe_groups = parser.GetAirframeGroups()
 
     # Output to XML file
     if args.xml:
         if args.verbose: print("Creating XML file " + args.xml)
-        out = xmlout.XMLOutput(param_groups, args.board)
+        out = xmlout.XMLOutput(airframe_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(param_groups, args.board, args.image_path)
+        out = markdownout.MarkdownTablesOutput(airframe_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(param_groups, args.board)
+        out = rcout.RCOutput(airframe_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(param_groups, args.board, post_start=True)
+        out_post = rcout.RCOutput(airframe_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,
-			.path = "/fs/mtd_mft",
+			.type = MTD_MFT_VER,
+			.path = "/fs/mtd_mft_ver",
 			.nblocks = 248
 		},
 		{
@@ -86,12 +86,17 @@ static const px4_mtd_entry_t base_eeprom = {
 
 static const px4_mtd_entry_t imu_eeprom = {
 	.device = &i2c4,
-	.npart = 2,
+	.npart = 3,
 	.partd = {
 		{
 			.type = MTD_CALDATA,
 			.path = "/fs/mtd_caldata",
-			.nblocks = 248
+			.nblocks = 240
+		},
+		{
+			.type = MTD_MFT_REV,
+			.path = "/fs/mtd_mft_rev",
+			.nblocks = 8
 		},
 		{
 			.type = MTD_ID,

boards/modalai/voxl2-slpi/default.px4board

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

boards/modalai/voxl2-slpi/src/CMakeLists.txt

@@ -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,25 +30,8 @@
 # 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.
-#
-############################################################################
 
-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)
+add_library(drivers_board
+	board_config.h
+	init.c
+    )

boards/modalai/voxl2-slpi/src/board_config.h

@@ -0,0 +1,50 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file board_config.h
+ *
+ * VOXL2 internal definitions
+ */
+
+#pragma once
+
+#define BOARD_HAS_NO_RESET
+#define BOARD_HAS_NO_BOOTLOADER
+/*
+ * I2C buses
+ */
+#define PX4_NUMBER_I2C_BUSES    3
+
+#include <system_config.h>
+#include <px4_platform_common/board_common.h>

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

@@ -0,0 +1,35 @@
+/****************************************************************************
+ *
+ * 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/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
+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
 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/install-voxl.sh
+px4$ boards/modalai/voxl2/scripts/install-voxl.sh
 ```
 
 ## Running PX4 on VOXL 2
@@ -66,10 +66,17 @@ 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-qurt.sh

@@ -1,11 +0,0 @@
-#!/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

@@ -0,0 +1,7 @@
+
+# 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_qurt.cmake

@@ -1,43 +0,0 @@
-############################################################################
-#
-# 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,3 +3,5 @@ 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/scripts/build-apps.sh

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

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

@@ -0,0 +1,11 @@
+#!/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/scripts/install-voxl.sh

@@ -1,19 +1,19 @@
 #!/bin/bash
 
-# Push qurt image to voxl2
-adb push build/modalai_voxl2_qurt/platforms/qurt/libpx4.so /usr/lib/rfsa/adsp
+# Push slpi image to voxl2
+adb push build/modalai_voxl2-slpi_default/platforms/qurt/libpx4.so /usr/lib/rfsa/adsp
 
-# Push posix image to voxl2
+# Push apps processor 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/voxl-px4 /usr/bin
+adb push boards/modalai/voxl2/target/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/voxl-px4.config /etc/modalai
+adb push boards/modalai/voxl2/target/voxl-px4.config /etc/modalai
 
 adb shell sync

boards/modalai/voxl2/src/board_config.h

@@ -42,10 +42,8 @@
 #define BOARD_HAS_NO_RESET
 #define BOARD_HAS_NO_BOOTLOADER
 
-/*
- * I2C buses
- */
-#define PX4_NUMBER_I2C_BUSES    3
+// Define this as empty since there are no I2C buses
+#define BOARD_I2C_BUS_CLOCK_INIT
 
 #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,
-			.path = "/fs/mtd_mft",
+			.type = MTD_MFT_VER,
+			.path = "/fs/mtd_mft_ver",
 			.nblocks = 248
 		},
 		{
@@ -86,12 +86,17 @@ static const px4_mtd_entry_t base_eeprom = {
 
 static const px4_mtd_entry_t imu_eeprom = {
 	.device = &i2c4,
-	.npart = 2,
+	.npart = 3,
 	.partd = {
 		{
 			.type = MTD_CALDATA,
 			.path = "/fs/mtd_caldata",
-			.nblocks = 248
+			.nblocks = 240
+		},
+		{
+			.type = MTD_MFT_REV,
+			.path = "/fs/mtd_mft_rev",
+			.nblocks = 8
 		},
 		{
 			.type = MTD_ID,

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

@@ -65,12 +65,17 @@ static const px4_mtd_entry_t fmum_fram = {
 
 static const px4_mtd_entry_t imu_eeprom = {
 	.device = &i2c4,
-	.npart = 2,
+	.npart = 3,
 	.partd = {
 		{
 			.type = MTD_CALDATA,
 			.path = "/fs/mtd_caldata",
-			.nblocks = 248
+			.nblocks = 240
+		},
+		{
+			.type = MTD_MFT_REV,
+			.path = "/fs/mtd_mft_rev",
+			.nblocks = 8
 		},
 		{
 			.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,
-			.path = "/fs/mtd_mft",
+			.type = MTD_MFT_VER,
+			.path = "/fs/mtd_mft_ver",
 			.nblocks = 248
 		},
 		{
@@ -86,12 +86,17 @@ static const px4_mtd_entry_t base_eeprom = {
 
 static const px4_mtd_entry_t imu_eeprom = {
 	.device = &i2c4,
-	.npart = 2,
+	.npart = 3,
 	.partd = {
 		{
 			.type = MTD_CALDATA,
 			.path = "/fs/mtd_caldata",
-			.nblocks = 248
+			.nblocks = 240
+		},
+		{
+			.type = MTD_MFT_REV,
+			.path = "/fs/mtd_mft_rev",
+			.nblocks = 8
 		},
 		{
 			.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,
-			.path = "/fs/mtd_mft",
+			.type = MTD_MFT_VER,
+			.path = "/fs/mtd_mft_ver",
 			.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 "qurt" OR ${LABEL} MATCHES "recovery" OR ${LABEL} MATCHES "bootloader" OR ${LABEL} MATCHES "canbootloader")
+    if(${LABEL} MATCHES "default" 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,10 +228,6 @@ 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,6 +170,9 @@ 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,7 +186,6 @@ 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, zero rotation is facing towards front of vehicle
+float32[4] q					# Attitude of the camera relative to NED earth-fixed frame when using a gimbal, otherwise vehicle attitude
 int8 result					# 1 for success, 0 for failure, -1 if camera does not provide feedback

msg/VehicleAngularAcceleration.msg

@@ -1,5 +0,0 @@
-
-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,7 +1,9 @@
 
-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		  # 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
 
 # TOPICS vehicle_angular_velocity vehicle_angular_velocity_groundtruth

platforms/common/CMakeLists.txt

@@ -33,7 +33,7 @@
 
 set(SRCS)
 
-if(NOT "${PX4_BOARD}" MATCHES "io-v2" AND NOT "${PX4_BOARD_LABEL}" MATCHES "bootloader")
+if(NOT "${PX4_PLATFORM}" MATCHES "qurt" AND 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

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

platforms/common/include/px4_platform_common/mtd_manifest.h

@@ -37,12 +37,13 @@ typedef enum  {
 	MTD_PARAMETERS  = 1,
 	MTD_WAYPOINTS   = 2,
 	MTD_CALDATA     = 3,
-	MTD_MFT         = 4,
-	MTD_ID          = 5,
-	MTD_NET         = 6,
+	MTD_MFT_VER     = 4,
+	MTD_MFT_REV     = 5,
+	MTD_ID          = 6,
+	MTD_NET         = 7
 } px4_mtd_types_t;
-#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"}
+#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"}
 
 typedef struct  {
 	const px4_mtd_types_t   type;

platforms/common/uORB/Kconfig

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

platforms/common/uORB/uORBDeviceMaster.cpp

@@ -36,10 +36,6 @@
 #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 ORB_COMMUNICATOR
+#ifdef CONFIG_ORB_COMMUNICATOR
 #include "uORBCommunicator.hpp"
-#endif /* ORB_COMMUNICATOR */
+#endif /* CONFIG_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 ORB_COMMUNICATOR
+#ifdef CONFIG_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 /* ORB_COMMUNICATOR */
+#endif /* CONFIG_ORB_COMMUNICATOR */
 
 	return PX4_OK;
 }
@@ -346,7 +346,7 @@ int uORB::DeviceNode::unadvertise(orb_advert_t handle)
 	return PX4_OK;
 }
 
-#ifdef ORB_COMMUNICATOR
+#ifdef CONFIG_ORB_COMMUNICATOR
 int16_t uORB::DeviceNode::topic_advertised(const orb_metadata *meta)
 {
 	uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
@@ -357,19 +357,7 @@ int16_t uORB::DeviceNode::topic_advertised(const orb_metadata *meta)
 
 	return -1;
 }
-
-/*
-//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 */
+#endif /* CONFIG_ORB_COMMUNICATOR */
 
 px4_pollevent_t
 uORB::DeviceNode::poll_state(cdev::file_t *filp)
@@ -413,7 +401,7 @@ void uORB::DeviceNode::add_internal_subscriber()
 	lock();
 	_subscriber_count++;
 
-#ifdef ORB_COMMUNICATOR
+#ifdef CONFIG_ORB_COMMUNICATOR
 	uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
 
 	if (ch != nullptr && _subscriber_count > 0) {
@@ -421,7 +409,7 @@ void uORB::DeviceNode::add_internal_subscriber()
 		ch->add_subscription(_meta->o_name, 1);
 
 	} else
-#endif /* ORB_COMMUNICATOR */
+#endif /* CONFIG_ORB_COMMUNICATOR */
 
 	{
 		unlock();
@@ -433,7 +421,7 @@ void uORB::DeviceNode::remove_internal_subscriber()
 	lock();
 	_subscriber_count--;
 
-#ifdef ORB_COMMUNICATOR
+#ifdef CONFIG_ORB_COMMUNICATOR
 	uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
 
 	if (ch != nullptr && _subscriber_count == 0) {
@@ -441,13 +429,13 @@ void uORB::DeviceNode::remove_internal_subscriber()
 		ch->remove_subscription(_meta->o_name);
 
 	} else
-#endif /* ORB_COMMUNICATOR */
+#endif /* CONFIG_ORB_COMMUNICATOR */
 	{
 		unlock();
 	}
 }
 
-#ifdef ORB_COMMUNICATOR
+#ifdef CONFIG_ORB_COMMUNICATOR
 int16_t uORB::DeviceNode::process_add_subscription(int32_t rateInHz)
 {
 	// if there is already data in the node, send this out to
@@ -490,7 +478,7 @@ int16_t uORB::DeviceNode::process_received_message(int32_t length, uint8_t *data
 
 	return PX4_OK;
 }
-#endif /* ORB_COMMUNICATOR */
+#endif /* CONFIG_ORB_COMMUNICATOR */
 
 int uORB::DeviceNode::update_queue_size(unsigned int queue_size)
 {

platforms/common/uORB/uORBDeviceNode.hpp

@@ -41,6 +41,7 @@
 #include <containers/IntrusiveSortedList.hpp>
 #include <containers/List.hpp>
 #include <px4_platform_common/atomic.h>
+#include <px4_platform_common/px4_config.h>
 
 namespace uORB
 {
@@ -122,9 +123,8 @@ public:
 
 	static int        unadvertise(orb_advert_t handle);
 
-#ifdef ORB_COMMUNICATOR
+#ifdef CONFIG_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 /* ORB_COMMUNICATOR */
+#endif /* CONFIG_ORB_COMMUNICATOR */
 
 	/**
 	  * Add the subscriber to the node's list of subscriber.  If there is

platforms/common/uORB/uORBManager.cpp

@@ -48,6 +48,10 @@
 #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()
@@ -258,7 +262,7 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
 		}
 	}
 
-#ifdef ORB_COMMUNICATOR
+#ifdef CONFIG_ORB_COMMUNICATOR
 
 	/*
 	 * Generate the path to the node and try to open it.
@@ -300,7 +304,7 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
 		}
 	}
 
-#endif /* ORB_COMMUNICATOR */
+#endif /* CONFIG_ORB_COMMUNICATOR */
 
 	return ret;
 }
@@ -360,10 +364,10 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta,
 		return nullptr;
 	}
 
-#ifdef ORB_COMMUNICATOR
+#ifdef CONFIG_ORB_COMMUNICATOR
 	// For remote systems call over and inform them
 	uORB::DeviceNode::topic_advertised(meta);
-#endif /* ORB_COMMUNICATOR */
+#endif /* CONFIG_ORB_COMMUNICATOR */
 
 	/* the advertiser may perform an initial publish to initialise the object */
 	if (data != nullptr) {
@@ -611,7 +615,7 @@ int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, i
 	return fd;
 }
 
-#ifdef ORB_COMMUNICATOR
+#ifdef CONFIG_ORB_COMMUNICATOR
 void uORB::Manager::set_uorb_communicator(uORBCommunicator::IChannel *channel)
 {
 	_comm_channel = channel;
@@ -623,18 +627,53 @@ void uORB::Manager::set_uorb_communicator(uORBCommunicator::IChannel *channel)
 
 uORBCommunicator::IChannel *uORB::Manager::get_uorb_communicator()
 {
-	return _comm_channel;
+	pthread_mutex_lock(&_communicator_mutex);
+	uORBCommunicator::IChannel *temp = _comm_channel;
+	pthread_mutex_unlock(&_communicator_mutex);
+
+	return temp;
 }
 
 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;
 
-	if (isAdvertisement) {
+	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);
 		_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;
@@ -723,7 +762,7 @@ bool uORB::Manager::is_remote_subscriber_present(const char *messageName)
 	return _remote_subscriber_topics.find(messageName);
 }
 
-#endif /* ORB_COMMUNICATOR */
+#endif /* CONFIG_ORB_COMMUNICATOR */
 
 #ifdef ORB_USE_PUBLISHER_RULES
 

platforms/common/uORB/uORBManager.hpp

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

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

@@ -35,6 +35,7 @@
 
 #if defined(CONFIG_SYSTEM_CDCACM)
 __BEGIN_DECLS
+#include <board_config.h>
 #include <arch/board/board.h>
 #include <syslog.h>
 #include <nuttx/wqueue.h>
@@ -42,6 +43,7 @@ __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,8 +32,10 @@
  ****************************************************************************/
 #pragma once
 
-#define HW_VERSION_EEPROM 		0x7		//!< Get hw_info from EEPROM
-#define HW_EEPROM_VERSION_MIN	0x10	//!< Minimum supported version
+#include <errno.h>
+
+#define HW_ID_EEPROM 			0x7		//!< Get hw_info from EEPROM
+#define HW_EEPROM_ID_MIN		0x10	//!< Minimum supported id (version/revision)
 
 #pragma pack(push, 1)
 
@@ -43,13 +45,13 @@ typedef struct {
 
 typedef struct {
 	mtd_mft_t version;
-	uint16_t hw_extended_ver;
+	uint16_t hw_extended_id; //<! HW version for MTD_MFT_VER, HW revision for MTD_MFT_REV
 	uint16_t crc;
 } mtd_mft_v0_t;
 
 typedef struct {
-	mtd_mft_t  version;
-	uint16_t hw_extended_ver;
+	mtd_mft_t version;
+	uint16_t hw_extended_id;
 	//{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,22 +445,36 @@ __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) {
 
-		if (rvmft == OK && path != NULL && hw_version == HW_VERSION_EEPROM) {
+		// 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) {
 
 			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_ver;
+				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;
 			}
 		}
 	}
@@ -495,14 +509,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("Verson is: %d, Only mft version %d is supported\n", 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);
 		return -EINVAL;
 	}
 
 	mtd_mft_v0_t *mtd_mft = (mtd_mft_v0_t *)mtd_mft_unk;
 
-	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);
+	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);
 		return -EINVAL;
 	}
 

platforms/posix/CMakeLists.txt

@@ -97,18 +97,14 @@ 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,12 +31,18 @@
 #
 ############################################################################
 
-set(module_libraries modules__muorb__slpi)
+get_property(module_libraries GLOBAL PROPERTY PX4_MODULE_LIBRARIES)
 
-QURT_LIB(LIB_NAME px4
-	SOURCES
-		${PX4_SOURCE_DIR}/platforms/qurt/unresolved_symbols.c
-	LINK_LIBS
-		${module_libraries}
-		px4_layer
+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
 	)

platforms/qurt/cmake/px4_impl_os.cmake

@@ -31,6 +31,27 @@
 #
 ############################################################################
 
+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,46 +176,3 @@ 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,6 +35,7 @@
 #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
 
-#ifdef __cplusplus
-extern "C" {
-#endif
+#include <pthread.h>
+#include <visibility.h>
+
+__BEGIN_DECLS
 
 typedef unsigned long useconds_t;
 int usleep(useconds_t usec);
+int pthread_setname_np(pthread_t __target_thread, const char *__name);
 
-#ifdef __cplusplus
-}
-#endif
+__END_DECLS

platforms/qurt/src/px4/CMakeLists.txt

@@ -35,6 +35,7 @@
 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

@@ -0,0 +1,39 @@
+/****************************************************************************
+ *
+ * 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,16 +180,6 @@ 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 = (time_t)abstime / 1000000;
-	abstime -= (hrt_abstime)(ts->tv_sec * 1000000);
+	ts->tv_sec = (typeof(ts->tv_sec))(abstime / 1000000);
+	abstime -= (hrt_abstime)(ts->tv_sec) * 1000000;
 	ts->tv_nsec = (typeof(ts->tv_nsec))(abstime * 1000);
 }
 

src/drivers/gps/gps.cpp

@@ -535,11 +535,12 @@ void GPS::handleInjectDataTopic()
 
 	bool updated = false;
 
-	// Limit maximum number of GPS injections to 6 since usually
+	// Limit maximum number of GPS injections to 8 since usually
 	// GPS injections should consist of 1-4 packets (GPS, Glonass, BeiDou, Galileo).
-	// Looking at 6 packets thus guarantees, that at least a full injection
+	// Looking at 8 packets thus guarantees, that at least a full injection
 	// data set is evaluated.
-	const size_t max_num_injections = 6;
+	// 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;
 	size_t num_injections = 0;
 
 	do {

src/drivers/rc/crsf_rc/CrsfRc.cpp

@@ -37,6 +37,7 @@
 
 #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,6 +31,7 @@
  *
  ****************************************************************************/
 
+#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_mdb = 0;
-	param_get(param_find("CANNODE_SUB_MDB"), &cannode_sub_mdb);
+	int32_t cannode_sub_mbd = 0;
+	param_get(param_find("CANNODE_SUB_MBD"), &cannode_sub_mbd);
 
-	if (cannode_sub_mdb == 1) {
+	if (cannode_sub_mbd == 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 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 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_point_valid = wp_msg.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid;
 	const bool avoidance_bezier_valid = bezier_msg.bezier_order > 0;
@@ -86,8 +86,12 @@ void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel
 	}
 
 	if (avoidance_invalid) {
-		PX4_WARN("Obstacle Avoidance system failed, loitering");
-		_publishVehicleCmdDoLoiter();
+		if (_avoidance_activated) {
+			// Invalid point received: deactivate
+			PX4_WARN("Obstacle Avoidance system failed, loitering");
+			_publishVehicleCmdDoLoiter();
+			_avoidance_activated = false;
+		}
 
 		if (!_failsafe_position.isAllFinite()) {
 			// save vehicle position when entering failsafe
@@ -98,13 +102,18 @@ 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 {
+	} else if (!_avoidance_activated) {
+		// First setpoint has been received: activate
+		PX4_INFO("Obstacle Avoidance system activated");
 		_failsafe_position.setNaN();
+		_avoidance_activated = true;
 	}
 
-	if (avoidance_point_valid) {
+	if (avoidance_point_valid && !wp_msg_timeout) {
 		const auto &point0 = wp_msg.waypoints[vehicle_trajectory_waypoint_s::POINT_0];
 		pos_sp = Vector3f(point0.position);
 		vel_sp = Vector3f(point0.velocity);
@@ -115,8 +124,7 @@ void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel
 			yaw_speed_sp = point0.yaw_speed;
 		}
 
-	} else if (avoidance_bezier_valid) {
-
+	} else if (avoidance_bezier_valid && !bezier_msg_timeout) {
 		float yaw = NAN, yaw_speed = NAN;
 		_generateBezierSetpoints(pos_sp, vel_sp, yaw, yaw_speed);
 

src/lib/avoidance/ObstacleAvoidance.hpp

@@ -130,6 +130,8 @@ 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,5 +259,16 @@ 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,6 +136,7 @@ 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);
@@ -164,13 +165,6 @@ 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,7 +50,6 @@
 #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>
@@ -105,7 +104,6 @@ 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,6 +33,8 @@
 
 #include "CameraFeedback.hpp"
 
+using namespace time_literals;
+
 CameraFeedback::CameraFeedback() :
 	ModuleParams(nullptr),
 	WorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
@@ -112,11 +114,39 @@ CameraFeedback::Run()
 		}
 
 		// Fill attitude data
-		// 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];
+		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];
+		}
+
 		capture.result = 1;
 
 		_capture_pub.publish(capture);

src/modules/camera_feedback/CameraFeedback.hpp

@@ -55,6 +55,7 @@
 #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
 {
@@ -81,6 +82,7 @@ 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,17 +1862,22 @@ 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 ((_param_takeoff_finished_action.get() == 1) && auto_mission_available) {
-				_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION);
+			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);
 
-			} else {
+				} 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.
 				_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER);
 			}
 		}

src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp

@@ -35,8 +35,7 @@
 
 HealthAndArmingChecks::HealthAndArmingChecks(ModuleParams *parent, vehicle_status_s &status)
 	: ModuleParams(parent),
-	  _context(status),
-	  _reporter(_failsafe_flags)
+	  _context(status)
 {
 	// 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:
-	Context _context;
-	Report _reporter;
-	orb_advert_t _mavlink_log_pub{nullptr};
-
 	failsafe_flags_s _failsafe_flags{};
 
+	Context _context;
+	Report _reporter{_failsafe_flags};
+	orb_advert_t _mavlink_log_pub{nullptr};
+
 	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,23 +142,6 @@ 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
  *
@@ -224,7 +207,8 @@ PARAM_DEFINE_INT32(COM_RC_ARM_HYST, 1000);
  *
  * @group Commander
  * @unit s
- * @decimal 2
+ * @decimal 1
+ * @increment 0.1
  */
 
 PARAM_DEFINE_FLOAT(COM_DISARM_LAND, 2.0f);
@@ -240,7 +224,8 @@ PARAM_DEFINE_FLOAT(COM_DISARM_LAND, 2.0f);
  *
  * @group Commander
  * @unit s
- * @decimal 2
+ * @decimal 1
+ * @increment 0.1
  */
 PARAM_DEFINE_FLOAT(COM_DISARM_PRFLT, 10.0f);
 
@@ -277,8 +262,6 @@ 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);
 
@@ -312,7 +295,6 @@ 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);
@@ -1033,6 +1015,8 @@ 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,6 +467,11 @@ 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,6 +92,7 @@ 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,6 +43,7 @@ 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,21 +66,23 @@ using math::Utilities::sq;
 using math::Utilities::updateYawInRotMat;
 
 // maximum sensor intervals in 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)
+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)
 
 // bad accelerometer detection and mitigation
-#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)
+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)
 
 // ground effect compensation
-#define GNDEFFECT_TIMEOUT       10E6    ///< Maximum period of time that ground effect protection will be active after it was last turned on (uSec)
+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)
 
 enum class VelocityFrame : uint8_t {
-	LOCAL_FRAME_FRD = 0,
-	BODY_FRAME_FRD  = 1
+	LOCAL_FRAME_NED = 0,
+	LOCAL_FRAME_FRD = 1,
+	BODY_FRAME_FRD  = 2
 };
 
 enum GeoDeclinationMask : uint8_t {
@@ -126,6 +128,13 @@ 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)
@@ -136,7 +145,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)
-	USE_EXT_VIS_VEL  = (1<<8),      ///< set to true to use external vision velocity data
+	DEPRECATED_USE_EXT_VIS_VEL = (1<<8), ///< set to true to use external vision velocity data
 };
 
 struct gpsMessage {
@@ -227,10 +236,11 @@ 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    velVar{};      ///< XYZ velocity variances ((m/sec)**2)
-	float       angVar{};      ///< angular heading variance (rad**2)
+	Vector3f    velocity_var{};    ///< XYZ velocity variances ((m/sec)**2)
+	Vector3f    orientation_var{}; ///< orientation 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 {
@@ -244,6 +254,14 @@ 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
@@ -265,6 +283,7 @@ 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
 
@@ -357,6 +376,9 @@ 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))
@@ -395,11 +417,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{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 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 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{5000000};     ///< amount of time spent inertial dead reckoning before the estimator reports the state estimates as invalid (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)
 
 	// static barometer pressure position error coefficient along body axes
 	float static_pressure_coef_xp{0.0f};    // (-)
@@ -434,7 +456,6 @@ 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,6 +47,23 @@ 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) {
@@ -86,7 +103,7 @@ void Ekf::controlFusionModes()
 	}
 
 	if (_gps_buffer) {
-		_gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GPS_MAX_INTERVAL);
+		_gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL);
 
 		// check for arrival of new sensor data at the fusion time horizon
 		_time_prev_gps_us = _gps_sample_delayed.time_us;
@@ -217,7 +234,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.fusion_mode & SensorFusionMask::USE_EXT_VIS_VEL)) && !_control_status.flags.ev_yaw) {
+				|| (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VEL))) && !_control_status.flags.ev_yaw) {
 
 			// rotate EV measurements into the EKF Navigation frame
 			calcExtVisRotMat();
@@ -232,11 +249,6 @@ 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();
-				}
 			}
 		}
 
@@ -331,15 +343,6 @@ 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();
 				}
 			}
@@ -354,38 +357,10 @@ 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.angVar, 1.e-4f);
+		const float ev_yaw_obs_var = fmaxf(_ev_sample_delayed.orientation_var(2), 1.e-4f);
 
 		if (PX4_ISFINITE(measured_hdg)) {
 			_aid_src_ev_yaw.timestamp_sample = _ev_sample_delayed.time_us;
@@ -414,12 +389,28 @@ 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
@@ -444,7 +435,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 * GPS_MAX_INTERVAL);
+		const bool is_gps_yaw_data_intermittent = !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, 2 * GNSS_YAW_MAX_INTERVAL);
 
 		const bool starting_conditions_passing = continuing_conditions_passing
 				&& _control_status.flags.tilt_align
@@ -638,10 +629,3 @@ 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,8 +204,6 @@ 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 { return _hagl_valid; };
+	bool isTerrainEstimateValid() const;
 
 	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.posD_counter; }
+	uint8_t get_posD_reset_count() const { return _state_reset_status.reset_count.posD; }
 	void get_posD_reset(float *delta, uint8_t *counter) const
 	{
 		*delta = _state_reset_status.posD_change;
-		*counter = _state_reset_status.posD_counter;
+		*counter = _state_reset_status.reset_count.posD;
 	}
 
 	// 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.velD_counter; }
+	uint8_t get_velD_reset_count() const { return _state_reset_status.reset_count.velD; }
 	void get_velD_reset(float *delta, uint8_t *counter) const
 	{
 		*delta = _state_reset_status.velD_change;
-		*counter = _state_reset_status.velD_counter;
+		*counter = _state_reset_status.reset_count.velD;
 	}
 
 	// 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.posNE_counter; }
+	uint8_t get_posNE_reset_count() const { return _state_reset_status.reset_count.posNE; }
 	void get_posNE_reset(float delta[2], uint8_t *counter) const
 	{
 		_state_reset_status.posNE_change.copyTo(delta);
-		*counter = _state_reset_status.posNE_counter;
+		*counter = _state_reset_status.reset_count.posNE;
 	}
 
 	// 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.velNE_counter; }
+	uint8_t get_velNE_reset_count() const { return _state_reset_status.reset_count.velNE; }
 	void get_velNE_reset(float delta[2], uint8_t *counter) const
 	{
 		_state_reset_status.velNE_change.copyTo(delta);
-		*counter = _state_reset_status.velNE_counter;
+		*counter = _state_reset_status.reset_count.velNE;
 	}
 
 	// 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.quat_counter; }
+	uint8_t get_quat_reset_count() const { return _state_reset_status.reset_count.quat; }
 	void get_quat_reset(float delta_quat[4], uint8_t *counter) const
 	{
 		_state_reset_status.quat_change.copyTo(delta_quat);
-		*counter = _state_reset_status.quat_counter;
+		*counter = _state_reset_status.reset_count.quat;
 	}
 
 	// get EKF innovation consistency check status information comprising of:
@@ -451,20 +451,27 @@ private:
 	void updateHorizontalDeadReckoningstatus();
 	void updateVerticalDeadReckoningStatus();
 
-	void updateTerrainValidity();
+	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)
+	};
 
-	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)
+	struct StateResets {
 		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
-	} _state_reset_status{};	///< reset event monitoring structure containing velocity, position, height and yaw reset information
+
+		StateResetCounts reset_count{};
+	};
+
+	StateResets _state_reset_status{};	///< reset event monitoring structure containing velocity, position, height and yaw reset information
+	StateResetCounts _state_reset_count_prev{};
 
 	Vector3f _ang_rate_delayed_raw{};	///< uncorrected angular rate vector at fusion time horizon (rad/sec)
 
@@ -504,6 +511,8 @@ 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)
@@ -628,7 +637,6 @@ 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
@@ -700,7 +708,6 @@ 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);
@@ -719,6 +726,8 @@ 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;
@@ -765,7 +774,6 @@ private:
 	void fuseFlowForTerrain();
 
 	void controlHaglFakeFusion();
-	void resetHaglFake();
 
 	// reset the heading and magnetic field states using the declination and magnetometer measurements
 	// return true if successful
@@ -784,10 +792,6 @@ 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>
@@ -872,6 +876,7 @@ 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();
@@ -881,7 +886,6 @@ 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);
@@ -1033,7 +1037,6 @@ private:
 	void stopGpsYawFusion();
 
 	void startEvPosFusion();
-	void startEvVelFusion();
 	void startEvYawFusion();
 
 	void stopEvFusion();
@@ -1067,9 +1070,6 @@ 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,13 +44,6 @@
 #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;
@@ -84,8 +77,16 @@ void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f
 
 	_output_new.vel.xy() += delta_horz_vel;
 
-	_state_reset_status.velNE_change = delta_horz_vel;
-	_state_reset_status.velNE_counter++;
+	// 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++;
 
 	// Reset the timout timer
 	_time_last_hor_vel_fuse = _imu_sample_delayed.time_us;
@@ -108,8 +109,16 @@ 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;
 
-	_state_reset_status.velD_change = delta_vert_vel;
-	_state_reset_status.velD_counter++;
+	// 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++;
 
 	// Reset the timout timer
 	_time_last_ver_vel_fuse = _imu_sample_delayed.time_us;
@@ -158,8 +167,16 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f
 
 	_output_new.pos.xy() += delta_horz_pos;
 
-	_state_reset_status.posNE_change = delta_horz_pos;
-	_state_reset_status.posNE_counter++;
+	// 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++;
 
 	// Reset the timout timer
 	_time_last_hor_pos_fuse = _imu_sample_delayed.time_us;
@@ -187,27 +204,36 @@ 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));
 	}
 
-	// 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++;
+	const float delta_z = new_vert_pos - old_vert_pos;
 
 	// 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) += _state_reset_status.posD_change;
+	_output_new.pos(2) += delta_z;
 
 	// 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) += _state_reset_status.posD_change;
-		_output_vert_buffer[i].vert_vel_integ += _state_reset_status.posD_change;
+		_output_buffer[i].pos(2) += delta_z;
+		_output_vert_buffer[i].vert_vel_integ += delta_z;
 	}
 
 	// add the reset amount to the output observer vertical position state
 	_output_vert_new.vert_vel_integ = _state.pos(2);
 
-	_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);
+	// 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);
 
 	// Reset the timout timer
 	_time_last_hgt_fuse = _imu_sample_delayed.time_us;
@@ -297,7 +323,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.angVar, sq(1.0e-2f));
+	const float yaw_new_variance = fmaxf(_ev_sample_delayed.orientation_var(2), sq(1.0e-2f));
 
 	resetQuatStateYaw(yaw_new, yaw_new_variance);
 	_R_ev_to_ekf.setIdentity();
@@ -863,6 +889,8 @@ 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;
@@ -1156,56 +1184,6 @@ 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()
 {
@@ -1368,14 +1346,6 @@ 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
@@ -1405,15 +1375,6 @@ 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) {
@@ -1456,9 +1417,6 @@ 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);
@@ -1466,17 +1424,26 @@ 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 = _state_reset_status.quat_change * _output_buffer[i].quat_nominal;
+		_output_buffer[i].quat_nominal = q_error * _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 = _state_reset_status.quat_change * _output_new.quat_nominal;
+	_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++;
 
 	_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
@@ -1508,9 +1475,6 @@ 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,6 +410,42 @@ 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
@@ -496,14 +532,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 | SensorFusionMask::USE_EXT_VIS_VEL)) {
+	if ((_params.fusion_mode & (SensorFusionMask::USE_EXT_VIS_POS | SensorFusionMask::USE_EXT_VIS_YAW)) || (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::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 = ceilf(max_time_delay_ms / filter_update_period_ms);
+	_imu_buffer_length = math::max(2, (int)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,6 +101,8 @@ 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; }
@@ -355,9 +357,10 @@ protected:
 	uint64_t _time_last_in_air{0};		///< last time we were in air (uSec)
 
 	// data buffer instances
-	RingBuffer<imuSample> _imu_buffer{12};           // buffer length 12 with default parameters
-	RingBuffer<outputSample> _output_buffer{12};
-	RingBuffer<outputVert> _output_vert_buffer{12};
+	static constexpr uint8_t kBufferLengthDefault = 12;
+	RingBuffer<imuSample> _imu_buffer{kBufferLengthDefault};
+	RingBuffer<outputSample> _output_buffer{kBufferLengthDefault};
+	RingBuffer<outputVert> _output_vert_buffer{kBufferLengthDefault};
 
 	RingBuffer<gpsSample> *_gps_buffer{nullptr};
 	RingBuffer<magSample> *_mag_buffer{nullptr};
@@ -368,6 +371,7 @@ 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,8 +94,28 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample)
 					bias_est.setBias(-_state.pos(2) + measurement);
 
 					// reset vertical velocity
-					if (PX4_ISFINITE(ev_sample.vel(2)) && (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_POS)) {
-						resetVerticalVelocityTo(ev_sample.vel(2), ev_sample.velVar(2));
+					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;
+						}
 
 					} else {
 						resetVerticalVelocityToZero();

src/modules/ekf2/EKF/ev_vel_control.cpp

@@ -0,0 +1,231 @@
+/****************************************************************************
+ *
+ *   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 * GPS_MAX_INTERVAL)
+				&& isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_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 * GPS_MAX_INTERVAL)) {
+		   && !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_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,25 +112,24 @@ void Ekf::controlGpsFusion()
 					fuseVelocity(_aid_src_gnss_vel);
 					fuseHorizontalPosition(_aid_src_gnss_pos);
 
-					if (shouldResetGpsFusion()) {
-						const bool was_gps_signal_lost = isTimedOut(_time_prev_gps_us, 1'000'000);
+					bool do_vel_pos_reset = shouldResetGpsFusion();
 
-						/* 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 (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.
 						 */
-						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 (resetYawToEKFGSF()) {
+							ECL_WARN("GPS emergency yaw reset");
+							do_vel_pos_reset = true;
 						}
+					}
 
+					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;
@@ -221,30 +220,20 @@ 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 is_reset_required = hasHorizontalAidingTimedOut()
+	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
 				       || 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_recent_takeoff_nav_failure || is_inflight_nav_failure);
+	return (is_reset_required || is_inflight_nav_failure);
 }
 
 bool Ekf::isYawFailure() const

src/modules/ekf2/EKF/mag_control.cpp

@@ -209,7 +209,6 @@ 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,6 +45,8 @@
 
 #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)
@@ -52,31 +54,8 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
 	resetEstimatorAidStatus(aid_src);
 	aid_src.timestamp_sample = _flow_sample_delayed.time_us;
 
-	// 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
+	const Vector2f vel_body = predictFlowVelBody();
+	const float range = predictFlowRange();
 
 	// 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
@@ -106,150 +85,24 @@ 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.
-	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));
+	float range = predictFlowRange();
 
-	// 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
+	const Vector24f state_vector = getStateAtFusionHorizonAsVector();
 
-	// 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>();
+	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);
 
-	// 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) {
+	if ((_aid_src_optical_flow.innovation_variance[0] < R_LOS)
+	    || (_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;
 	}
-	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));
@@ -265,75 +118,37 @@ void Ekf::fuseOptFlow()
 	bool fused[2] {false, false};
 
 	// fuse observation axes sequentially
-	{
-		// 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;
+	for (uint8_t index = 0; index <= 1; index++) {
+		if (index == 0) {
+			// everything was already computed above
 
-		// 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;
+		} 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);
 
-		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));
+			// 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;
+			}
 		}
 
-		if (measurementUpdate(Kfusion, Hfusion, _aid_src_optical_flow.innovation[0])) {
-			fused[0] = true;
-			_fault_status.flags.bad_optflow_X = false;
+		SparseVector24f<0,1,2,3,4,5,6> Hfusion(H);
+		Vector24f Kfusion = P * Hfusion / _aid_src_optical_flow.innovation_variance[index];
 
-		} else {
-			_fault_status.flags.bad_optflow_X = true;
-			return;
+		if (measurementUpdate(Kfusion, Hfusion, _aid_src_optical_flow.innovation[index])) {
+			fused[index] = true;
 		}
 	}
 
-	{
-		// 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;
-		}
-	}
+	_fault_status.flags.bad_optflow_X = !fused[0];
+	_fault_status.flags.bad_optflow_Y = !fused[1];
 
 	if (fused[0] && fused[1]) {
 		_aid_src_optical_flow.time_last_fuse = _imu_sample_delayed.time_us;
@@ -341,6 +156,39 @@ 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,6 +337,56 @@ 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"])
@@ -352,3 +402,5 @@ 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

@@ -0,0 +1,127 @@
+// -----------------------------------------------------------------------------
+// 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

@@ -0,0 +1,95 @@
+// -----------------------------------------------------------------------------
+// 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

@@ -1,640 +0,0 @@
-#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

@@ -1,184 +0,0 @@
-// 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

@@ -1,157 +0,0 @@
-// 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,7 +45,14 @@
 
 void Ekf::initHagl()
 {
-	resetHaglFake();
+	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);
 }
 
 void Ekf::runTerrainEstimator()
@@ -66,8 +73,6 @@ 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()
@@ -306,7 +311,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++;
 }
@@ -410,27 +415,23 @@ void Ekf::controlHaglFakeFusion()
 	if (!_control_status.flags.in_air
 	    && !_hagl_sensor_status.flags.range_finder
 	    && !_hagl_sensor_status.flags.flow) {
-		resetHaglFake();
+
+		initHagl();
 	}
 }
 
-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()
+bool Ekf::isTerrainEstimateValid() const
 {
 	// we have been fusing range finder measurements in the last 5 seconds
-	const bool recent_range_fusion = isRecent(_time_last_hagl_fuse, (uint64_t)5e6);
+	if (_hagl_sensor_status.flags.range_finder && isRecent(_time_last_hagl_fuse, (uint64_t)5e6)) {
+		return true;
+	}
 
 	// 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
-	const bool recent_flow_for_terrain_fusion = isRecent(_time_last_flow_terrain_fuse, (uint64_t)5e6);
+	if (_hagl_sensor_status.flags.flow && isRecent(_time_last_flow_terrain_fuse, (uint64_t)5e6)) {
+		return true;
+	}
 
-	_hagl_valid = (recent_range_fusion || recent_flow_for_terrain_fusion);
+	return false;
 }