Add tailsitter heading alignment phase during forward transition (Pr yaw alignment) #39
There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -986,6 +986,133 @@ 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 = 2000; // 20.0 degrees | ||
| const float MAX_SPIN_RATE_DEG = 8.0f; // Max yaw rate allowed | ||
| const uint32_t ALIGN_PHASE_LIMIT_MS = 6000; // 6s Total Timeout | ||
| const uint32_t WAIT_DELAY_MS = 1000; // 1s 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; | ||
| static int32_t target_bearing_cd = 0; | ||
| static bool alignment_completed_for_this_flight = false; | ||
| static bool target_bearing_latched = 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 | ||
| target_bearing_latched = false; | ||
| target_bearing_cd = 0; | ||
| } | ||
| last_run_ms = now_; | ||
|
greptile-apps[bot] marked this conversation as resolved.
|
||
|
|
||
| // 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) { | ||
| if (!target_bearing_latched) { | ||
| target_bearing_cd = plane.prev_WP_loc.get_bearing_to(plane.next_WP_loc); | ||
| quadplane.attitude_control->reset_rate_controller_I_terms(); | ||
| gcs().send_text(MAV_SEVERITY_INFO, "Alignment start: Target Heading %.1f", | ||
| target_bearing_cd * 0.01f); | ||
| target_bearing_latched = true; | ||
| } | ||
|
greptile-apps[bot] marked this conversation as resolved.
|
||
| int32_t current_yaw_cd = quadplane.ahrs_view->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.x); | ||
|
|
||
| // 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); | ||
|
|
||
|
Comment on lines
+1007
to
+1059
There was a problem hiding this comment. Start the alignment timeout only when alignment is actually active. 💡 One way to gate the timer- // --- DETECT NEW FLIGHT/TRANSITION ENTRY ---
- // If this function hasn't run for >200ms, assume it's a new attempt.
+ // --- DETECT NEW FLIGHT/TRANSITION ENTRY ---
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) {
+ align_phase_start_ms = 0;
+ alignment_done_ms = 0;
+ } else if (align_phase_start_ms == 0) {
+ align_phase_start_ms = now_;
+ }🤖 Prompt for AI Agents |
||
| // --- BLOCKING CONTROL LOOP --- | ||
| // Run this ONLY if we are NOT done waiting AND haven't timed out | ||
| if (!wait_complete && !timeout_expired) { | ||
|
|
||
| // Log (2Hz) | ||
| if (now_ - last_log_ms > 500) { | ||
| 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", abs(error_cd) * 0.01f); | ||
| } | ||
| last_log_ms = now_; | ||
| } | ||
| //Force zero climb rate (Altitude Hold) | ||
| quadplane.set_climb_rate_cms(0); | ||
|
|
||
| // Control | ||
| quadplane.pos_control->update_z_controller(); | ||
|
There was a problem hiding this comment. We are manually calling ardupilot/ArduPlane/quadplane.cpp Line 1040 in a095c6c Here is a relevant PR from when the comment was added: ArduPilot#19286 There was a problem hiding this comment. Can try calling Will try this when I have time. |
||
| 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, | ||
| false | ||
| ); | ||
|
|
||
| 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(); | ||
|
greptile-apps[bot] marked this conversation as resolved.
|
||
| } | ||
| } | ||
|
|
||
| // Normal transition code continues here for non-tailsitters | ||
| // or after tailsitter alignment completes... | ||
| if (tailsitter.transition_fw_complete()) { | ||
| // To inform the attitude controller that FW_DONE | ||
| quadplane.attitude_control->set_tailsitter_transition(false); | ||
|
|
@@ -1050,8 +1177,6 @@ void Tailsitter_Transition::VTOL_update() | |
| if (!quadplane.tailsitter.transition_vtol_complete()) { | ||
| return; | ||
| } | ||
| // transition to VTOL complete, if armed set vtol rate limit starting point | ||
| if (plane.arming.is_armed_and_safety_off()) { | ||
| vtol_limit_start_ms = now; | ||
|
|
||
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
millis()call and confusingnow_shadow variablenowis already declared at line 973 for the same timestamp. Introducing a secondnow_from a separateAP_HAL::millis()call is a minor timing inconsistency and creates a confusing naming shadow throughout the alignment block. Use the existingnowvariable directly.Note: If this suggestion doesn't match your team's coding style, reply to this and let me know. I'll remember it for next time!