diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index 2df80fbbc1415c..8da8ecc0cd4008 100644 --- a/ArduPlane/Log.cpp +++ b/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 { diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index de33fcd130c392..cc997d75c074af 100644 --- a/ArduPlane/tailsitter.cpp +++ b/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()) { diff --git a/ArduPlane/version.h b/ArduPlane/version.h index c595533f6c17ea..97030e63004be7 100644 --- a/ArduPlane/version.h +++ b/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 diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index c68e143a112fb1..b6422c57d124ef 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/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. diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index f75230980414d2..e7d72e1cd64d23 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/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; diff --git a/libraries/AP_AHRS/AP_AHRS_Logging.cpp b/libraries/AP_AHRS/AP_AHRS_Logging.cpp index a5d77476238d53..f20bb8383e011b 100644 --- a/libraries/AP_AHRS/AP_AHRS_Logging.cpp +++ b/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 diff --git a/libraries/AP_AHRS/AP_AHRS_View.cpp b/libraries/AP_AHRS/AP_AHRS_View.cpp index 16833e6a74dab6..8fc51e1dd9d5a7 100644 --- a/libraries/AP_AHRS/AP_AHRS_View.cpp +++ b/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; diff --git a/libraries/AP_AHRS/AP_AHRS_View.h b/libraries/AP_AHRS/AP_AHRS_View.h index 1ad748062f5023..50347705627668 100644 --- a/libraries/AP_AHRS/AP_AHRS_View.h +++ b/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 diff --git a/libraries/AP_AHRS/LogStructure.h b/libraries/AP_AHRS/LogStructure.h index 618ade0a5c78cb..8cd0fe00274578 100644 --- a/libraries/AP_AHRS/LogStructure.h +++ b/libraries/AP_AHRS/LogStructure.h @@ -9,7 +9,12 @@ LOG_ORGN_MSG, \ LOG_POS_MSG, \ LOG_RATE_MSG, \ - LOG_ATSC_MSG + LOG_ATSC_MSG, \ + LOG_ATT_PIT_COMP_MSG, \ + LOG_ATT_TAR_AA_MSG, \ + LOG_ATT_BOD_AA_MSG, \ + LOG_ATT_ERR_AA_MSG + // @LoggerMessage: AHR2 // @Description: Backup AHRS data @@ -74,6 +79,68 @@ struct PACKED log_Attitude { uint8_t active; }; +// @LoggerMessage: ATPC +// @Description: Canonical vehicle attitude pitch compensation +// @Field: TimeUS: Time since system startup +// @Field: AttPNew: vehicle pitch post compensation for horizon crossing +// @Field: AttPOld: vehicle pitch +// @Field: ZNedZ: Body Z vector's Z component in NED frame +// @Field: XNedZ: Body X vector's Z component in NED frame +// @Field: isNoseDn: Boolean to track if craft is nose down below horizon +// @Field: isBellyUp: Boolean to track if craft is belly up or down +struct PACKED log_AttitudeViewCompensation { + LOG_PACKET_HEADER; + uint64_t time_us; + int16_t pitch; + int16_t pitch_raw; + float z_ned_z; + float x_ned_z; + float is_nose_down; + float is_belly_up; +}; + +// @LoggerMessage: ATAT +// @Description: Canonical vehicle attitude target axis angle representation of body to NED quat +// @Field: TimeUS: Time since system startup +// @Field: TarX: X component of axis angle vector +// @Field: TarY: Y component of axis angle vector +// @Field: TarZ: Z component of axis angle vector +struct PACKED log_AttitudeTarAxisAng { + LOG_PACKET_HEADER; + uint64_t time_us; + float target_x; + float target_y; + float target_z; +}; + +// @LoggerMessage: ATAB +// @Description: Canonical vehicle attitude body axis angle representation of body to NED quat +// @Field: TimeUS: Time since system startup +// @Field: BodyX: X component of axis angle vector +// @Field: BodyY: Y component of axis angle vector +// @Field: BodyZ: Z component of axis angle vector +struct PACKED log_AttitudeBodyAxisAng { + LOG_PACKET_HEADER; + uint64_t time_us; + float body_x; + float body_y; + float body_z; +}; + +// @LoggerMessage: ATAE +// @Description: Canonical vehicle attitude error axis angle representation of body to NED quat +// @Field: TimeUS: Time since system startup +// @Field: ErrX: X component of axis angle vector +// @Field: ErrY: Y component of axis angle vector +// @Field: ErrZ: Z component of axis angle vector +struct PACKED log_AttitudeErrAxisAng { + LOG_PACKET_HEADER; + uint64_t time_us; + float err_x; + float err_y; + float err_z; +}; + // @LoggerMessage: ORGN // @Description: Vehicle navigation origin or other notable position // @Field: TimeUS: Time since system startup @@ -209,5 +276,13 @@ struct PACKED log_ATSC { { LOG_ATSC_MSG, sizeof(log_ATSC), \ "ATSC", "Qffffff", "TimeUS,AngPScX,AngPScY,AngPScZ,PDScX,PDScY,PDScZ", "s------", "F000000" , true }, \ { LOG_VIDEO_STABILISATION_MSG, sizeof(log_Video_Stabilisation), \ - "VSTB", "Qffffffffff", "TimeUS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,Q1,Q2,Q3,Q4", "sEEEooo----", "F0000000000" }, + "VSTB", "Qffffffffff", "TimeUS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,Q1,Q2,Q3,Q4", "sEEEooo----", "F0000000000" }, \ + { LOG_ATT_PIT_COMP_MSG, sizeof(log_AttitudeViewCompensation), \ + "ATPC", "Qccffff", "TimeUS,AttPNew,AttPOld,ZNedZ,XNedZ,isNoseDn,isBellyUp", "sdd----", "FBB0000" }, \ + { LOG_ATT_TAR_AA_MSG, sizeof(log_AttitudeTarAxisAng), \ + "ATAT", "Qfff", "TimeUS,TarX,TarY,TarZ", "s---", "F000" }, \ + { LOG_ATT_BOD_AA_MSG, sizeof(log_AttitudeBodyAxisAng), \ + "ATAB", "Qfff", "TimeUS,BodyX,BodyY,BodyZ", "s---", "F000" }, \ + { LOG_ATT_ERR_AA_MSG, sizeof(log_AttitudeErrAxisAng), \ + "ATAE", "Qfff", "TimeUS,ErrX,ErrY,ErrZ", "s---", "F000" }, diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6C-bdshot/scripts/ab_door_auto_control.lua b/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6C-bdshot/scripts/ab_door_auto_control.lua index 9da3a05e5ebc13..3bc361619e8b1d 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6C-bdshot/scripts/ab_door_auto_control.lua +++ b/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6C-bdshot/scripts/ab_door_auto_control.lua @@ -11,15 +11,32 @@ local CONFIG_UPDATE_INTERVAL_MS = 5000 local RC_SWITCH_THRESHOLD = 1500 local PWM_SAFE_MIN = 800 local PWM_SAFE_MAX = 2200 -local MAV_CMD_NAV_VTOL_LAND = 85 -local MAV_CMD_NAV_LAND = 21 + +-- Modes for landing +local QMODES = { + [17] = true, -- QSTABILIZE + [18] = true, -- QHOVER + [19] = true, -- QLOITER + [20] = true, -- QLAND + [21] = true, -- QRTL + [22] = true, -- QAUTOTUNE + [23] = true, -- QACRO +} + +-- Mission commands for landing phase +local NAV_CMDS = { + [17] = true, -- MAV_CMD_NAV_LOITER_UNLIM + [19] = true, -- MAV_CMD_NAV_LOITER_TIME + [31] = true, -- MAV_CMD_NAV_LOITER_TO_ALT + [85] = true, -- MAV_CMD_NAV_VTOL_LAND +} -- ############################################################################### -- # PARAMETER DEFINITIONS -- ############################################################################### local PARAM_TABLE_KEY = 88 local PARAM_PREFIX = "DOOR_" -local NUM_PARAMS = 18 +local NUM_PARAMS = 19 assert(param:add_table(PARAM_TABLE_KEY, PARAM_PREFIX, NUM_PARAMS), "Failed to create param table") local PARAM_IDX = { @@ -39,7 +56,10 @@ local PARAM_IDX = { S4_FUNC = 15, S4_OPEN = 16, S4_CLOSE = 17, -- Slew rate parameter (18) - SLEW_RATE = 18 + SLEW_RATE = 18, + + -- OVERRIDE (19) + OVERRIDE = 19 } --[[ @Param: DOOR_MAN_CMD_CH, @DisplayName: Door Manual Command Chan, @Range: 1 16 --]] @@ -54,16 +74,24 @@ assert(param:add_param(PARAM_TABLE_KEY, PARAM_IDX.VTOL_PITCH, "VTOL_PITCH", 75)) assert(param:add_param(PARAM_TABLE_KEY, PARAM_IDX.FW_PITCH, "FW_PITCH", 25)) -- Servo function and PWM parameters +local open_pwms = {1000, 1900, 1000, 1900} +local closed_pwms = {1900, 1000, 1900, 1000} + for i=1,4 do assert(param:add_param(PARAM_TABLE_KEY, PARAM_IDX['S'..i..'_FUNC'], "S"..i.."_FUNC", 105+i)) - assert(param:add_param(PARAM_TABLE_KEY, PARAM_IDX['S'..i..'_OPEN'], "S"..i.."_OPEN", 1100)) - assert(param:add_param(PARAM_TABLE_KEY, PARAM_IDX['S'..i..'_CLOSE'], "S"..i.."_CLOSE", 1900)) + assert(param:add_param(PARAM_TABLE_KEY, PARAM_IDX['S'..i..'_OPEN'], "S"..i.."_OPEN", open_pwms[i])) + assert(param:add_param(PARAM_TABLE_KEY, PARAM_IDX['S'..i..'_CLOSE'], "S"..i.."_CLOSE", closed_pwms[i])) end -- Slew rate parameter definition --[[ @Param: DOOR_SLEW_RATE, @DisplayName: Door Servo Slew Rate, @Description: Speed of servo movement in PWM units per second. 0 disables slew and makes movement instant., @Units: PWM/s, @Range: 0 1000, @User: Standard --]] assert(param:add_param(PARAM_TABLE_KEY, PARAM_IDX.SLEW_RATE, "SLEW_RATE", 1800)) +-- Override parameter definition +--[[ @Param: DOOR_OVERRIDE, @DisplayName: Door Servo override, @Description: Enable/Disable for doors script auto logic, @Range: 0 1 --]] +assert(param:add_param(PARAM_TABLE_KEY, PARAM_IDX.OVERRIDE, "OVERRIDE", 0)) + + -- ############################################################################### -- # SCRIPT INITIALIZATION & LOGIC @@ -79,6 +107,7 @@ local state = { was_in_fw_state = false, last_config_update = 0, was_armed = false, + landing_latch = false, -- State table for each servo's position servos = {} } @@ -89,7 +118,8 @@ local config = { alt_trig_m = Parameter(PARAM_PREFIX .. "ALT_TRIG_M"), vtol_pitch = Parameter(PARAM_PREFIX .. "VTOL_PITCH"), fw_pitch = Parameter(PARAM_PREFIX .. "FW_PITCH"), - slew_rate = Parameter(PARAM_PREFIX .. "SLEW_RATE") + slew_rate = Parameter(PARAM_PREFIX .. "SLEW_RATE"), + man_override = Parameter(PARAM_PREFIX .. "OVERRIDE") } local servos_config = {} @@ -120,7 +150,8 @@ function update_config_cache() alt_trig_m = config.alt_trig_m:get(), vtol_pitch = config.vtol_pitch:get(), fw_pitch = config.fw_pitch:get(), - slew_rate = config.slew_rate:get() + slew_rate = config.slew_rate:get(), + man_override = config.man_override:get() } if config.cache.man_cmd_ch < 1 or config.cache.man_cmd_ch > 16 then @@ -240,23 +271,29 @@ function get_tailsitter_flight_state() else return "TRANSITIONING" end end -function is_in_landing_phase(current_mode, flight_state) - local MODES = { qland = 20, land = 21, rtl = 11, auto = 10 } - if not current_mode or not flight_state then return false end +function is_in_landing_phase(current_mode) + local MODES = { auto = 10 } -- other valid modes which need additional checks + if not current_mode then return false end - if (current_mode == MODES.qland or current_mode == MODES.land or current_mode == MODES.rtl) then - if flight_state == "VTOL" then - return true - end + -- Return true for all q modes directly, regardless of pitch + if QMODES[current_mode] then + return true end + -- Check for vertical mission commands for auto mode if current_mode == MODES.auto then local nav_cmd = mission:get_current_nav_id() - if nav_cmd == MAV_CMD_NAV_VTOL_LAND and flight_state == "VTOL" then + if NAV_CMDS[nav_cmd] then + -- valid command for landing phase return true end end + -- Check VTOL descent flag + if quadplane and quadplane:in_vtol_land_descent() then + return true + end + return false end @@ -272,13 +309,8 @@ function run_auto_mode() local is_currently_in_vtol = (flight_state == "VTOL") local is_currently_in_fw = (flight_state == "FIXED_WING") - if is_in_landing_phase(current_mode, flight_state) then - if state.doors_are_commanded_closed then - log_message(MAV_SEVERITY.WARNING, "Landing confirmed - opening doors") - open_all_doors() - state.takeoff_climbout_complete = false - end - elseif not state.takeoff_climbout_complete then + -- Phase 1 - Check for takeoff + if not state.takeoff_climbout_complete then if is_currently_in_vtol then local alt = get_current_altitude_agl() if alt and alt >= config.cache.alt_trig_m then @@ -286,20 +318,38 @@ function run_auto_mode() if close_all_doors() then state.takeoff_climbout_complete = true end + else + -- Force doors open under alt_trig_m + open_all_doors() end end - else - if state.was_in_fw_state and is_currently_in_vtol then - log_message(MAV_SEVERITY.INFO, "FW->VTOL transition detected. Opening doors.") - open_all_doors() - end + return + end - if state.was_in_vtol_state and is_currently_in_fw then - log_message(MAV_SEVERITY.INFO, "VTOL->FW transition detected. Closing doors.") - close_all_doors() - end + -- Phase 2 - Checks during flight and landing + -- DETECT TRANSITION (FW -> VTOL) to set the Landing Latch + if state.was_in_fw_state and is_currently_in_vtol then + state.landing_latch = true + log_message(MAV_SEVERITY.INFO, "Transition to VTOL detected. Doors OPEN.") end + -- SAFETY: We DO NOT clear latch on (VTOL -> FW) transition anymore, + -- because instability can look like a FW transition + -- APPLY STATE BASED ON MODES + if is_in_landing_phase(current_mode) then + if state.landing_latch then + -- We are latched OPEN. + -- Stays TRUE even if pitch goes to 0 (Fix for instability safety). + open_all_doors() + end -- latch checking + else + -- Non-VTOL Modes (FW, FBWA, MANUAL, etc.) + -- Auto mode with mission commands other than Loiter/nav_vtol_land or other landing command ids + -- Force Latch Reset here and close doors + state.landing_latch = false + close_all_doors() + end -- landing phase checking + if flight_state ~= "TRANSITIONING" then state.was_in_vtol_state = is_currently_in_vtol state.was_in_fw_state = is_currently_in_fw @@ -307,6 +357,7 @@ function run_auto_mode() end function check_manual_override() + local rc_pwm = rc:get_pwm(config.cache.man_cmd_ch) if not rc_pwm then return false end @@ -318,7 +369,7 @@ function check_manual_override() state.last_rc_pwm = rc_pwm if state.is_in_manual_override then - if config.cache.man_timeout > 0 and (millis() - state.manual_override_timer) > (config.cache.man_timeout * 1000) then + if config.cache.man_override == 0 and config.cache.man_timeout > 0 and (millis() - state.manual_override_timer) > (config.cache.man_timeout * 1000) then log_message(MAV_SEVERITY.WARNING, "Manual override timeout") state.is_in_manual_override = false return false @@ -368,6 +419,7 @@ function update() state.takeoff_climbout_complete = false state.was_in_vtol_state = false state.was_in_fw_state = false + state.landing_latch = false elseif not is_armed_now and state.was_armed then log_message(MAV_SEVERITY.INFO, "Disarmed. Resetting state and opening doors.") open_all_doors() @@ -375,6 +427,7 @@ function update() state.takeoff_climbout_complete = false state.was_in_vtol_state = false state.was_in_fw_state = false + state.landing_latch = false end state.was_armed = is_armed_now diff --git a/modules/littlefs b/modules/littlefs new file mode 160000 index 00000000000000..34be692aafaaf4 --- /dev/null +++ b/modules/littlefs @@ -0,0 +1 @@ +Subproject commit 34be692aafaaf4319a18e415b08aa4332697408b