diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 0181f392d804b..cbe2ed3e23f5d 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/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; } diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index e4489fd0d17c1..984f31a1ccb3a 100644 --- a/ArduPlane/quadplane.h +++ b/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; diff --git a/Tools/autotest/logger_metadata/parse.py b/Tools/autotest/logger_metadata/parse.py index 2f7517d2182f4..14dae90d3761d 100755 --- a/Tools/autotest/logger_metadata/parse.py +++ b/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)): diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index 0e6ef03d3f6c2..94de3f05d8182 100644 --- a/Tools/autotest/quadplane.py +++ b/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 diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index 3cab908b5951c..52f5b82aada30 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -3145,7 +3145,7 @@ def LoggerDocumentation(self): # 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'] diff --git a/libraries/AP_DAL/AP_DAL.h b/libraries/AP_DAL/AP_DAL.h index 64a16fff45608..4abe8952289fc 100644 --- a/libraries/AP_DAL/AP_DAL.h +++ b/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 diff --git a/libraries/AP_DAL/LogStructure.h b/libraries/AP_DAL/LogStructure.h index c8ef343584438..031ac1bba5914 100644 --- a/libraries/AP_DAL/LogStructure.h +++ b/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), \