Plane: Enable quadplane dead reckoning navigation to start earlier

ArduPlane/ArduPlane.cpp

@@ -520,12 +520,14 @@ void Plane::update_fly_forward(void)
             return;
         }
 
-        if (quadplane.in_vtol_mode()) {
-            ahrs.set_fly_forward(false);
+        if (quadplane.option_is_set(QuadPlane::OPTION::ASSUME_FLYING_FORWARD_IN_TRANSITION) &&
+            quadplane.in_transition()) {
+            ahrs.set_fly_forward(true);
             return;
         }
 
-        if (quadplane.in_assisted_flight()) {
+        if (quadplane.in_vtol_mode() ||
+            quadplane.in_assisted_flight()) {
             ahrs.set_fly_forward(false);
             return;
         }

ArduPlane/quadplane.h

@@ -604,6 +604,7 @@ class QuadPlane
         FS_RTL=(1<<20),
         DISARMED_TILT_UP=(1<<21),
         SCALE_FF_ANGLE_P=(1<<22),
+        ASSUME_FLYING_FORWARD_IN_TRANSITION=(1<<23),
     };
     bool option_is_set(OPTION option) const {
         return (options.get() & int32_t(option)) != 0;

Tools/autotest/logger_metadata/parse.py

@@ -171,7 +171,7 @@ def set_fmts(self, fmts):
                 return
             # Make sure lengths match up
             if len(fmts) != len(self.fields_order):
-                print(f"Number of fmts don't match fields: msg={self.name} fmts={fmts} num_fields={len(self.fields_order)}")
+                print(f"Number of fmts don't match fields: msg={self.name} fmts={fmts}:({len(fmts)=}) {self.fields_order=}:{len(self.fields_order)}")
                 return
             # Loop through the list
             for idx in range(0,len(fmts)):

Tools/autotest/quadplane.py

@@ -2072,6 +2072,66 @@ def WindEstimateConsistency(self):
                 self.progress("Wind estimates correlated")
                 break
 
+    def ASSUME_FLYING_FORWARD_IN_TRANSITION(self):
+        '''test the QuadPlane option for assumption of fly-forward in transition'''
+        self.set_parameters({
+            "LOG_DISARMED": 1,
+            "LOG_REPLAY": 1,
+        })
+        self.reboot_sitl()
+        self.wait_ready_to_arm()
+
+        self.set_parameters({
+            "RTL_AUTOLAND": 2,
+        })
+
+        # when RTL is entered and RTL_AUTOLAND is 1 we should fly home
+        # then to the landing sequence.  This mission puts the landing
+        # sequence well to the West of home so if we go directly there
+        # we won't come within 200m of home
+        wps = self.create_simple_relhome_mission([
+            # (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 30),
+            # fly North
+            (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 500, 0, 30),
+            # add a waypoint 1km North (which we will look for and trigger RTL
+            (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 1000, 0, 30),
+
+            # *exciting* landing sequence is ~1km West and points away from Home.
+            self.create_MISSION_ITEM_INT(
+                mavutil.mavlink.MAV_CMD_DO_LAND_START,
+            ),
+            (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, -1000, 30),
+            (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, -1300, 15),
+            (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, -1600, 5),
+            (mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND, 0, -1750, 0),
+        ])
+        self.check_mission_upload_download(wps)
+
+        self.set_parameters({
+            'GPS1_TYPE': 0,
+            'Q_OPTIONS': 1 << 23,  # ASSUME_FLYING_FORWARD_IN_TRANSITION
+        })
+        self.delay_sim_time(4)
+        self.change_mode('QHOVER')
+        self.arm_vehicle(force=True)
+        # self.set_parameter("SIM_SPEEDUP", 5)
+        self.set_rc(3, 1700)
+        start_alt = self.get_altitude(altitude_source='SIM_STATE.alt')
+        want_alt = start_alt + 100
+        self.wait_altitude(want_alt-10, want_alt+10, timeout=120)
+        self.set_rc(3, 1500)
+        self.change_mode('CRUISE')
+        self.delay_sim_time(10)
+        self.change_mode('AUTO')
+        self.wait_current_waypoint(self.get_mission_count()-1, timeout=500)
+
+        # does not auto-disarm; so will force it:
+        #        self.wait_disarmed(timeout=300)
+
+        self.wait_altitude(345, 350)
+        self.disarm_vehicle(force=True)
+        self.reboot_sitl()
+
     def tests(self):
         '''return list of all tests'''
 
@@ -2123,5 +2183,6 @@ def tests(self):
             self.RTL_AUTOLAND_1,  # as in fly-home then go to landing sequence
             self.RTL_AUTOLAND_1_FROM_GUIDED,  # as in fly-home then go to landing sequence
             self.AHRSFlyForwardFlag,
+            self.ASSUME_FLYING_FORWARD_IN_TRANSITION,
         ])
         return ret

Tools/autotest/vehicle_test_suite.py

@@ -3145,7 +3145,7 @@
         # we allow for no docs for replay messages, as these are not for end-users. They are
         # effectively binary blobs for replay
         REPLAY_MSGS = ['RFRH', 'RFRF', 'REV2', 'RSO2', 'RWA2', 'REV3', 'RSO3', 'RWA3', 'RMGI',
-                       'REY3', 'RFRN', 'RISH', 'RISI', 'RISJ', 'RBRH', 'RBRI', 'RRNH', 'RRNI',
+                       'REY3', 'RISH', 'RISI', 'RISJ', 'RBRH', 'RBRI', 'RRNH', 'RRNI',
                        'RGPH', 'RGPI', 'RGPJ', 'RASH', 'RASI', 'RBCH', 'RBCI', 'RVOH', 'RMGH',
                        'ROFH', 'REPH', 'REVH', 'RWOH', 'RBOH', 'RSLL']
 
@@ -3197,7 +3197,7 @@
                     raise NotAchievedException("documented field %s.%s not found in code" %
                                                (name, label))
         if len(missing) > 0:
             raise NotAchievedException("Documented messages (%s) not in code" % missing)
 
     def initialise_after_reboot_sitl(self):
 
@@ -3816,7 +3816,7 @@
                 if m.num_logs != num_logs:
                     raise NotAchievedException("Number of logs changed")
                 if m.time_utc < 1000 and m.id != m.num_logs:
                     raise NotAchievedException("Bad timestamp")
                 if m.id != m.last_log_num:
                     if m.size == 0:
                         raise NotAchievedException("Zero-sized log")

libraries/AP_DAL/AP_DAL.h

@@ -87,14 +87,21 @@ class AP_DAL {
     void log_writeDefaultAirSpeed3(const float aspeed, const float uncertainty);
     void log_writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t timeStamp_ms, uint8_t type);
 
-    enum class StateMask {
+    enum class RFRNFlags {
         ARMED = (1U<<0),
+        UNUSED = (1U<<1),
+        FLY_FORWARD = (1U<<2),
+        AHRS_AIRSPEED_SENSOR_ENABLED = (1U<<3),
+        OPTICALFLOW_ENABLED = (1U<<4),
+        WHEELENCODER_ENABLED = (1U<<5),
+        TAKEOFF_EXPECTED = (1U<<6),
+        TOUCHDOWN_EXPECTED = (1U<<7),
     };
 
     // EKF ID for timing checks
     enum class EKFType : uint8_t {
-        EKF2 = 0,
-        EKF3 = 1,
+        EKF2 = 2,
+        EKF3 = 3,
     };
 
     // check if we are low on CPU for this core

libraries/AP_DAL/LogStructure.h

@@ -58,6 +58,19 @@ struct log_RFRF {
 
 // @LoggerMessage: RFRN
 // @Description: Replay FRame - aNother frame header
+// @Field: HLat: home latitude
+// @Field: HLng: home latitude
+// @Field: HAlt: home altitude AMSL
+// @Field: E2T: EAS to TAS factor
+// @Field: AM: available memory
+// @Field: TX: AHRS trim X
+// @Field: TY: AHRS trim Y
+// @Field: TZ: AHRS trim Z
+// @Field: VC: AHRS Vehicle Class
+// @Field: EKT: configure EKF type
+// @FieldValueEnum: EKT: AP_DAL::EKFType
+// @Field: Flags: bitmask of boolean state
+// @FieldBitmaskEnum: Flags: AP_DAL::RFRNFlags
 struct log_RFRN {
     int32_t lat;
     int32_t lng;
@@ -392,7 +405,7 @@ struct log_RBOH {
     { LOG_RFRF_MSG, RLOG_SIZE(RFRF),                          \
       "RFRF", "BB", "FTypes,Slow", "--", "--" }, \
     { LOG_RFRN_MSG, RLOG_SIZE(RFRN),                            \
-      "RFRN", "IIIfIfffBBB", "HLat,HLon,HAlt,E2T,AM,TX,TY,TZ,VC,EKT,Flags", "DUm????????", "GGB--------" }, \
+      "RFRN", "IIIfIfffBBB", "HLat,HLon,HAlt,E2T,AM,TX,TY,TZ,VC,EKT,Flags", "DUm-bddd---", "GGB--------" }, \
     { LOG_REV2_MSG, RLOG_SIZE(REV2),                                   \
       "REV2", "B", "Event", "-", "-" }, \
     { LOG_RSO2_MSG, RLOG_SIZE(RSO2),                         \