Add tailsitter heading alignment phase during forward transition (Pr yaw alignment)

ArduPlane/Log.cpp

@@ -20,6 +20,18 @@ void Plane::Log_Write_Attitude(void)
         quadplane.attitude_control->get_attitude_target_quat().to_euler(targets.x, targets.y, targets.z);
         targets *= degrees(100.0f);
         quadplane.ahrs_view->Write_AttitudeView(targets);
+
+        // Logging for att target quat to axis angle, error quat to axis angle
+        Vector3f att_body_axis_ang, att_tar_axis_ang, att_err_axis_ang; 
+        quadplane.attitude_control->get_attitude_target_quat().to_axis_angle(att_tar_axis_ang);
+        quadplane.attitude_control->get_attitude_body_quat().to_axis_angle(att_body_axis_ang);
+        quadplane.attitude_control->get_attitude_error_quat().to_axis_angle(att_err_axis_ang);
+
+        quadplane.ahrs_view->Write_Att_Tar_Axis_Ang(att_tar_axis_ang);
+        quadplane.ahrs_view->Write_Att_Body_Axis_Ang(att_body_axis_ang);
+        quadplane.ahrs_view->Write_Att_Err_Axis_Ang(att_err_axis_ang);
+
+        quadplane.ahrs_view->Write_AttitudePitchCompensation();
     } else
 #endif
             {

ArduPlane/tailsitter.cpp

@@ -903,6 +903,139 @@ void Tailsitter_Transition::update()
     switch (transition_state) {
 
     case TRANSITION_ANGLE_WAIT_FW: {
+
+        const uint32_t now_ = AP_HAL::millis();
+        // -----------------------------------------------------------
+        // 1. INITIALIZATION
+        // -----------------------------------------------------------
+        quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
+
+        // -----------------------------------------------------------
+        // 2. TAILSITTER HEADING ALIGNMENT & WAIT
+        // -----------------------------------------------------------
+        if (quadplane.tailsitter.enabled()) {
+
+            // Constants
+            const int32_t  ALIGN_TOLERANCE_CD   = 1000;   // 10.0 degrees
+            const float    MAX_SPIN_RATE_DEG    = 3.0f;  // Max yaw rate allowed
+            const uint32_t ALIGN_PHASE_LIMIT_MS = 10000; // 10s Total Timeout
+            const uint32_t WAIT_DELAY_MS        = 2000;  // 2s Wait
+
+            // Static Variables (State Tracking)
+            static uint32_t align_phase_start_ms = 0;
+            static uint32_t alignment_done_ms = 0;
+            static uint32_t last_run_ms = 0;
+            static uint32_t last_log_ms = 0;
+            // NEW: One-Shot Flag to prevent infinite loops
+            static bool alignment_completed_for_this_flight = false; 
+
+            // --- DETECT NEW FLIGHT/TRANSITION ENTRY ---
+            // If this function hasn't run for >200ms, assume it's a new attempt.
+            if (now_ - last_run_ms > 200) {
+                align_phase_start_ms = now_;
+                alignment_done_ms = 0;
+                last_log_ms = 0;
+                alignment_completed_for_this_flight = false; // Reset flag for new transition
+            }
+            last_run_ms = now_;
+
+            // Check Validity & Only run if we haven't finished aligning yet
+            bool should_run_alignment = !alignment_completed_for_this_flight &&
+                                        (plane.control_mode == &plane.mode_auto || plane.control_mode == &plane.mode_guided) &&
+                                        (plane.nav_controller != nullptr);
+
+            if (should_run_alignment) {
+                // Disable weathervane during alignment
+                if (quadplane.weathervane != nullptr) {
+                    quadplane.weathervane->allow_weathervaning(false);
+                }
+
+                int32_t target_bearing_cd = plane.nav_controller->target_bearing_cd();
+                int32_t current_yaw_cd = quadplane.ahrs.yaw_sensor;
+                int32_t error_cd = wrap_180_cd(target_bearing_cd - current_yaw_cd);
+                Vector3f gyro = quadplane.ahrs.get_gyro();
+                float yaw_rate_deg = degrees(gyro.z); 
+
+                // LOGIC: Are we aligned right now?
+                bool is_aligned = (abs(error_cd) <= ALIGN_TOLERANCE_CD) && (abs(yaw_rate_deg) <= MAX_SPIN_RATE_DEG);
+
+                // TIMER LOGIC
+                if (is_aligned) {
+                    if (alignment_done_ms == 0) alignment_done_ms = now_;
+                } else {
+                    alignment_done_ms = 0; // Reset if we drift out
+                }
+
+                // EXIT CRITERIA
+                bool wait_complete = (alignment_done_ms != 0) && (now_ - alignment_done_ms >= WAIT_DELAY_MS);
+                bool timeout_expired = (now_ - align_phase_start_ms >= ALIGN_PHASE_LIMIT_MS);
+
+                // --- BLOCKING CONTROL LOOP ---
+                // Run this ONLY if we are NOT done waiting AND haven't timed out
+                if (!wait_complete && !timeout_expired) {
+
+                    // Log (1Hz)
+                    if (now_ - last_log_ms > 1000) { 
+                        if (is_aligned) {
+                             float remaining = (WAIT_DELAY_MS - (now_ - alignment_done_ms)) * 0.001f;
+                             gcs().send_text(MAV_SEVERITY_INFO, "Aligned. Waiting: %.1fs", (double)remaining);
+                        } else {
+                             gcs().send_text(MAV_SEVERITY_INFO, "Aligning: Err %.1f", error_cd * 0.01f);
+                        }
+                        last_log_ms = now_;
+                    }
+                    //Force zero climb rate (Altitude Hold)
+                    quadplane.pos_control->set_pos_target_z_from_climb_rate_cm(0.0f);
+
+                    // Control
+                    quadplane.pos_control->update_z_controller();
+                    quadplane.pos_control->update_xy_controller();
+                    quadplane.attitude_control->input_euler_angle_roll_pitch_yaw(
+                        quadplane.pos_control->get_roll_cd(),
+                        quadplane.pos_control->get_pitch_cd(),
+                        (float)target_bearing_cd,
+                        true
+                    );
+
+                    quadplane.motors_output();
+                    set_last_fw_pitch(); // Keep tracking pitch so we don't snap later
+                    return; // BLOCK TRANSITION
+                }
+
+                // --- HANDOVER LOGIC (Runs ONCE when done) ---
+
+                if (timeout_expired) {
+                     gcs().send_text(MAV_SEVERITY_WARNING, "Align Timeout: Proceeding");
+                } else {
+                     gcs().send_text(MAV_SEVERITY_INFO, "Alignment Complete: Transitioning");
+                }
+
+                // 1. Mark as complete so we NEVER enter this 'if' block again for this flight
+                alignment_completed_for_this_flight = true;
+
+                // 2. Reset Standard Transition Timer to now
+                fw_transition_start_ms = now_;
+
+                // 3. Reset Integrators and Pitch Target
+                quadplane.attitude_control->reset_rate_controller_I_terms();
+                plane.nav_pitch_cd = constrain_float(quadplane.ahrs.pitch_sensor, -8500, 8500);
+                plane.nav_roll_cd = 0;
+                set_last_fw_pitch();
+                // --- RE-ENABLE WEATHERVANE AFTER ALIGNMENT ---
+                if (quadplane.weathervane != nullptr) {
+                    quadplane.weathervane->allow_weathervaning(true);
+                }
+            }
+             else {
+                // --- ENSURE WEATHERVANE IS ENABLED WHEN NOT ALIGNING ---
+                if (quadplane.weathervane != nullptr) {
+                    quadplane.weathervane->allow_weathervaning(true);
+                }    
+            }
+        }
+
+        // Normal transition code continues here for non-tailsitters
+        // or after tailsitter alignment completes...
         if (tailsitter.transition_fw_complete()) {
             transition_state = TRANSITION_DONE;
             if (plane.arming.is_armed_and_safety_off()) {

ArduPlane/version.h

@@ -6,7 +6,7 @@
 
 #include "ap_version.h"
 
-#define THISFIRMWARE "Airbound ArduPlane V4.5.7.2"
+#define THISFIRMWARE "Airbound ArduPlane V4.5.7.2 - hf1 (Auto Yaw Align)"
 
 // the following line is parsed by the autotest scripts
 #define FIRMWARE_VERSION 4,5,7,FIRMWARE_VERSION_TYPE_DEV

libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

@@ -753,6 +753,7 @@ void AC_AttitudeControl::attitude_controller_run_quat()
 
     // Record error to handle EKF resets
     _attitude_ang_error = attitude_body.inverse() * _attitude_target;
+    _attitude_body = attitude_body;
 }
 
 // thrust_heading_rotation_angles - calculates two ordered rotations to move the attitude_body quaternion to the attitude_target quaternion.

libraries/AC_AttitudeControl/AC_AttitudeControl.h

@@ -226,6 +226,12 @@ class AC_AttitudeControl {
     // Return the angle between the target thrust vector and the current thrust vector.
     float get_att_error_angle_deg() const { return degrees(_thrust_error_angle); }
 
+    // Return the body-to-NED error attitude used by the quadplane-specific attitude control input methods
+    Quaternion get_attitude_error_quat() const { return _attitude_ang_error; }
+
+    // Return the body-to-NED attitude used by the quadplane-specific attitude control input methods
+    Quaternion get_attitude_body_quat() const { return _attitude_body; }
+
     // Set x-axis angular velocity in centidegrees/s
     void rate_bf_roll_target(float rate_cds) { _ang_vel_body.x = radians(rate_cds * 0.01f); }
 
@@ -498,6 +504,9 @@ class AC_AttitudeControl {
     // This represents a quaternion attitude error in the body frame, used for inertial frame reset handling.
     Quaternion          _attitude_ang_error;
 
+    // This represents a quaternion attitude in the body frame
+    Quaternion          _attitude_body;
+
     // The angle between the target thrust vector and the current thrust vector.
     float               _thrust_angle;
 

libraries/AP_AHRS/AP_AHRS_Logging.cpp

@@ -143,6 +143,60 @@ void AP_AHRS_View::Write_AttitudeView(const Vector3f &targets) const
     AP::logger().WriteBlock(&pkt, sizeof(pkt));
 }
 
+// Write an Attitude pitch compensation packet
+void AP_AHRS_View::Write_AttitudePitchCompensation() const
+{
+    const struct log_AttitudeViewCompensation pkt{
+        LOG_PACKET_HEADER_INIT(LOG_ATT_PIT_COMP_MSG),
+        time_us              : AP_HAL::micros64(),
+        pitch                : (int16_t)(degrees(pitch) * 100),
+        pitch_raw            : (int16_t)(degrees(pitch_raw) * 100),
+        z_ned_z              : z_ned.z,
+        x_ned_z              : x_ned.z,
+        is_nose_down         : (float)is_nose_down,
+        is_belly_up          : (float)is_belly_up,
+    };
+    AP::logger().WriteBlock(&pkt, sizeof(pkt));
+}
+
+// Write attitude target axis angle
+void AP_AHRS_View::Write_Att_Tar_Axis_Ang(Vector3f att_tar_axis_ang) const
+{
+    const struct log_AttitudeTarAxisAng pkt{
+        LOG_PACKET_HEADER_INIT(LOG_ATT_TAR_AA_MSG),
+        time_us                   : AP_HAL::micros64(),
+        target_x                  : att_tar_axis_ang.x,
+        target_y                  : att_tar_axis_ang.y,
+        target_z                  : att_tar_axis_ang.z,
+    };
+    AP::logger().WriteBlock(&pkt, sizeof(pkt));
+}
+
+// Write attitude body axis angle
+void AP_AHRS_View::Write_Att_Body_Axis_Ang(Vector3f att_body_axis_ang) const
+{
+    const struct log_AttitudeBodyAxisAng pkt{
+        LOG_PACKET_HEADER_INIT(LOG_ATT_BOD_AA_MSG),
+        time_us                 : AP_HAL::micros64(),
+        body_x                  : att_body_axis_ang.x,
+        body_y                  : att_body_axis_ang.y,
+        body_z                  : att_body_axis_ang.z,
+    };
+    AP::logger().WriteBlock(&pkt, sizeof(pkt));
+}
+
+// Write attitude error axis angle
+void AP_AHRS_View::Write_Att_Err_Axis_Ang(Vector3f att_err_axis_ang) const
+{
+    const struct log_AttitudeErrAxisAng pkt{
+        LOG_PACKET_HEADER_INIT(LOG_ATT_ERR_AA_MSG),
+        time_us                : AP_HAL::micros64(),
+        err_x                  : att_err_axis_ang.x,
+        err_y                  : att_err_axis_ang.y,
+        err_z                  : att_err_axis_ang.z,
+    };
+    AP::logger().WriteBlock(&pkt, sizeof(pkt));
+}
 // Write a rate packet
 void AP_AHRS_View::Write_Rate(const AP_Motors &motors, const AC_AttitudeControl &attitude_control,
                                 const AC_PosControl &pos_control) const

libraries/AP_AHRS/AP_AHRS_View.cpp

@@ -70,23 +70,31 @@ void AP_AHRS_View::update()
 
     rot_body_to_ned.to_euler(&roll, &pitch, &yaw);
 
+    pitch_raw = pitch;
+
     Vector3f z_body{0, 0, 1};
     Vector3f x_body{1, 0, 0};
-    Vector3f z_ned = rot_body_to_ned * z_body;
-    Vector3f x_ned = rot_body_to_ned * x_body;
 
+    z_ned = rot_body_to_ned * z_body;
+    x_ned = rot_body_to_ned * x_body;
 
     // compensate for going beyond +/- 90 degree in pitch
     if(z_ned.z <= 0 ){
         // Nose down below horizon
+        is_nose_down = true;
         if(x_ned.z > 0.0f) {
             // Belly down
             pitch = -M_PI - pitch;
+            is_belly_up = false;
         }
         else {
             // Belly up
             pitch = M_PI - pitch;
+            is_belly_up = true;
         }
+    } else {
+        is_nose_down = false;
+        is_belly_up = false;
     }
 
     roll_sensor  = degrees(roll) * 100;

libraries/AP_AHRS/AP_AHRS_View.h

@@ -174,17 +174,25 @@ class AP_AHRS_View
     }
 
     // Logging Functions
-    void Write_AttitudeView(const Vector3f &targets) const;    
+    void Write_AttitudeView(const Vector3f &targets) const;
+    void Write_AttitudePitchCompensation(void) const;    
+
+    void Write_Att_Tar_Axis_Ang(Vector3f att_tar_axis_ang) const;
+    void Write_Att_Body_Axis_Ang(Vector3f att_body_axis_ang) const;
+    void Write_Att_Err_Axis_Ang(Vector3f att_err_axis_ang) const;
+
     void Write_Rate( const AP_Motors &motors, const AC_AttitudeControl &attitude_control,
                         const AC_PosControl &pos_control) const;
 
     float roll;
-    float pitch;
+    float pitch, pitch_raw;
     float yaw;
     int32_t roll_sensor;
     int32_t pitch_sensor;
     int32_t yaw_sensor;
 
+    Vector3f z_ned, x_ned;
+    bool is_nose_down = false, is_belly_up = false;
 
     // get current rotation
     // note that this may not be the rotation were actually using, see _pitch_trim_deg