From b61029fe82d205a41dd3bbd9d27a1bc77617ec4c Mon Sep 17 00:00:00 2001 From: akshar-airbound Date: Tue, 30 Dec 2025 17:05:09 +0530 Subject: [PATCH 01/31] Modify Pixhawk6C-bdshot default parameter list - Update version.h to rc4 - Update default params in lua scripts --- ArduPlane/version.h | 2 +- .../hwdef/Pixhawk6C-bdshot/defaults.parm | 35 ++++++------------- .../scripts/ab_door_auto_control.lua | 2 +- .../scripts/ab_para_disarm.lua | 4 +-- 4 files changed, 14 insertions(+), 29 deletions(-) diff --git a/ArduPlane/version.h b/ArduPlane/version.h index bfa6d81b0f1e9b..1343a40324d2f5 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 - rc3" +#define THISFIRMWARE "Airbound ArduPlane V4.5.7.2 - rc4" // the following line is parsed by the autotest scripts #define FIRMWARE_VERSION 4,5,7,FIRMWARE_VERSION_TYPE_DEV diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6C-bdshot/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6C-bdshot/defaults.parm index de937bec8f5a77..cffcb572c6b3de 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6C-bdshot/defaults.parm +++ b/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6C-bdshot/defaults.parm @@ -36,21 +36,12 @@ COMPASS_EXTERNAL 1 COMPASS_OFFS_MAX 600 COMPASS_ORIENT 6 COMPASS_ORIENT2 37 -COMPASS_PRIO1_ID 658953 -COMPASS_PRIO2_ID 658961 -COMPASS_PRIO3_ID 658433 COMPASS_USE 1 COMPASS_USE2 1 COMPASS_USE3 0 CRASH_ACC_THRESH 25 CRASH_DETECT 1 -DOOR_ALT_TRIG_M 3 -DOOR_FW_PITCH 25 -DOOR_MAN_CMD_CH 9 -DOOR_MAN_TIMEOUT 5 -DOOR_SLEW_RATE 1800 -DOOR_VTOL_PITCH 75 -EK3_ALT_M_NSE 0.3 +EK3_ALT_M_NSE 3 FBWB_CLIMB_RATE 7 FLAP_SLEWRATE 125 FLIGHT_OPTIONS 768 @@ -73,19 +64,12 @@ GPS_POS2_X 0.164 GPS_POS2_Y -0.136 GPS_POS2_Z -0.01 GPS_PRIMARY 1 +GPS_RATE_MS 100 +GPS_RATE_MS2 100 GPS_TYPE2 1 -INS_ACC_ID 3408650 -INS_ACC1_CALTEMP 44.80676 -INS_ACC2_CALTEMP 43.5 -INS_ACC2_ID 2687498 INS_ACCEL_FILTER 10 INS_ENABLE_MASK 127 INS_FAST_SAMPLE 1 -INS_GYR_CAL 1 -INS_GYR_ID 3408650 -INS_GYR1_CALTEMP 24.27536 -INS_GYR2_CALTEMP 11.25 -INS_GYR2_ID 2687242 INS_GYRO_FILTER 50 INS_HNTCH_ATT 25 INS_HNTCH_BW 30 @@ -107,16 +91,13 @@ INS_POS2_Z -0.065 INS_RAW_LOG_OPT 9 LOG_FILE_DSRMROT 1 LOG_FILE_BUFSIZE 80 +LOG_FILE_MB_FREE 1000 LAND_DISARMDELAY 1 MIS_RESTART 1 MIS_TOTAL 7 NAVL1_DAMPING 0.78 NAVL1_PERIOD 15 ONESHOT_MASK 240 -PARA_DELAY_MS 100 -PARA_PWM_HOLD 2000 -PARA_PWM_RELEASE 1100 -PARA_TRIG_CH 11 PTCH_LIM_MAX_DEG 30 PTCH_LIM_MIN_DEG -10 PTCH_RATE_D 0.0009553 @@ -131,6 +112,8 @@ PTCH_RATE_P 0.1446696 PTCH_RATE_PDMX 0 PTCH_RATE_SMAX 150 PTCH_TRIM_DEG 3.5 +PTCH2SRV_RMAX_DN 90 +PTCH2SRV_RMAX_UP 90 PTCH2SRV_TCONST 0.45 Q_A_ACCEL_P_MAX 50000 Q_A_ACCEL_R_MAX 58651.07 @@ -211,7 +194,7 @@ Q_TAILSIT_VHPOW 1 Q_TAILSIT_WV_HI 92 Q_TAILSIT_WV_LO 25 Q_TAILSIT_WV_MGN 3 -Q_TAILSIT_WV_MI 26 +Q_TAILSIT_WV_MI 45 Q_THROTTLE_EXPO 0 Q_TRANS_FAIL 5 Q_TRANS_FAIL_ACT -1 @@ -241,6 +224,8 @@ RLL_RATE_FLTT 5.305164 RLL_RATE_I 0.1750721 RLL_RATE_IMAX 0.666 RLL_RATE_P 0.1750721 +RLL2SRV_RMAX 90 +RLL2SRV_TCONST 0.3 ROLL_LIMIT_DEG 40 RPM1_ESC_MASK 3072 RPM1_TYPE 5 @@ -258,7 +243,6 @@ SERVO_BLH_BDMASK 3072 SERVO_BLH_MASK 3072 SERVO_BLH_OTYPE 5 SERVO_BLH_POLES 28 -SERVO_BLH_RVMASK 1024 SERVO_DSHOT_ESC 3 SERVO1_FUNCTION 2 SERVO1_REVERSED 1 @@ -284,6 +268,7 @@ SERVO7_FUNCTION 77 SERVO7_REVERSED 1 SERVO8_FUNCTION 75 SERVO8_REVERSED 1 +SERVO_BLH_TRATE 100 SR0_ADSB 1 SR0_EXT_STAT 2 SR0_EXTRA1 4 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 bca81984440dc3..9da3a05e5ebc13 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 @@ -62,7 +62,7 @@ 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", 400)) +assert(param:add_param(PARAM_TABLE_KEY, PARAM_IDX.SLEW_RATE, "SLEW_RATE", 1800)) -- ############################################################################### diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6C-bdshot/scripts/ab_para_disarm.lua b/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6C-bdshot/scripts/ab_para_disarm.lua index 8d98a69ad79c48..c7239a0c9d8dbd 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6C-bdshot/scripts/ab_para_disarm.lua +++ b/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6C-bdshot/scripts/ab_para_disarm.lua @@ -15,8 +15,8 @@ local PARAM_TABLE_KEY = 72 assert(param:add_table(PARAM_TABLE_KEY, "PARA_", 4), "Failed to create param table") assert(param:add_param(PARAM_TABLE_KEY, 1, "DELAY_MS", 100), "Failed to create Param1") assert(param:add_param(PARAM_TABLE_KEY, 2, "TRIG_CH", 11), "Failed to create Param2") -assert(param:add_param(PARAM_TABLE_KEY, 3, "PWM_HOLD", 2000), "Failed to create Param3") -assert(param:add_param(PARAM_TABLE_KEY, 4, "PWM_RELEASE", 1000), "Failed to create Param4") +assert(param:add_param(PARAM_TABLE_KEY, 3, "PWM_HOLD", 1900), "Failed to create Param3") +assert(param:add_param(PARAM_TABLE_KEY, 4, "PWM_RELEASE", 1100), "Failed to create Param4") local PARA_DELAY_MS = Parameter() PARA_DELAY_MS:init("PARA_DELAY_MS") From 120698419dbba47fb60ee69bbf67f3c16f82eea6 Mon Sep 17 00:00:00 2001 From: Mayank Joneja Date: Wed, 7 Jan 2026 11:20:11 +0530 Subject: [PATCH 02/31] Handle gimbal lock singularity specifically for pitch in ahrs view --- libraries/AP_AHRS/AP_AHRS_View.cpp | 23 ++++++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) diff --git a/libraries/AP_AHRS/AP_AHRS_View.cpp b/libraries/AP_AHRS/AP_AHRS_View.cpp index 450567ce2c6238..d3110448a02dbc 100644 --- a/libraries/AP_AHRS/AP_AHRS_View.cpp +++ b/libraries/AP_AHRS/AP_AHRS_View.cpp @@ -68,7 +68,28 @@ void AP_AHRS_View::update() gyro = rot_view * gyro; } - rot_body_to_ned.to_euler(&roll, &pitch, &yaw); + // compute full-range pitch and fall back for singularity + const float ax = rot_body_to_ned.a.x; + const float bx = rot_body_to_ned.b.x; + const float cx = rot_body_to_ned.c.x; + + // safe denominator (cos(pitch) approx) + const float denom = sqrtf(ax*ax + bx*bx); + + if (denom > 1e-6f) { + // full-range pitch using atan2(sin,cos) -> returns (-PI, PI) + pitch = atan2F(-cx, denom); + // roll and yaw as before + roll = atan2F(rot_body_to_ned.c.y, rot_body_to_ned.c.z); + yaw = atan2F(rot_body_to_ned.b.x, rot_body_to_ned.a.x); + } else { + // near gimbal lock: denom ~ 0 => cos(pitch) ~ 0 (pitch ~ +/-90°) + // choose a stable convention: set roll to 0 and recover yaw from another pair + pitch = (cx > 0) ? -M_PI_2 : M_PI_2; // match existing sign convention + roll = 0.0f; + // use an alternate element to estimate yaw in singular case + yaw = atan2F(-rot_body_to_ned.a.y, rot_body_to_ned.b.y); + } roll_sensor = degrees(roll) * 100; pitch_sensor = degrees(pitch) * 100; From d6d469ab95878dc429f6d4f46c5b6e600aaea6b7 Mon Sep 17 00:00:00 2001 From: Mayank Joneja Date: Wed, 7 Jan 2026 11:20:44 +0530 Subject: [PATCH 03/31] Update version to rc5 --- ArduPlane/version.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduPlane/version.h b/ArduPlane/version.h index 1343a40324d2f5..975fc439de1a56 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 - rc4" +#define THISFIRMWARE "Airbound ArduPlane V4.5.7.2 - rc5" // the following line is parsed by the autotest scripts #define FIRMWARE_VERSION 4,5,7,FIRMWARE_VERSION_TYPE_DEV From 9bae2ea4ff2a369e86cb506e049a48609724fee1 Mon Sep 17 00:00:00 2001 From: Mayank Joneja Date: Wed, 7 Jan 2026 16:07:39 +0530 Subject: [PATCH 04/31] Add simple ahrs pitch based check to correct att pitch beyond 90 --- libraries/AP_AHRS/AP_AHRS_View.cpp | 31 ++++++++++-------------------- 1 file changed, 10 insertions(+), 21 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_View.cpp b/libraries/AP_AHRS/AP_AHRS_View.cpp index d3110448a02dbc..09fc3f1d42559a 100644 --- a/libraries/AP_AHRS/AP_AHRS_View.cpp +++ b/libraries/AP_AHRS/AP_AHRS_View.cpp @@ -68,27 +68,16 @@ void AP_AHRS_View::update() gyro = rot_view * gyro; } - // compute full-range pitch and fall back for singularity - const float ax = rot_body_to_ned.a.x; - const float bx = rot_body_to_ned.b.x; - const float cx = rot_body_to_ned.c.x; - - // safe denominator (cos(pitch) approx) - const float denom = sqrtf(ax*ax + bx*bx); - - if (denom > 1e-6f) { - // full-range pitch using atan2(sin,cos) -> returns (-PI, PI) - pitch = atan2F(-cx, denom); - // roll and yaw as before - roll = atan2F(rot_body_to_ned.c.y, rot_body_to_ned.c.z); - yaw = atan2F(rot_body_to_ned.b.x, rot_body_to_ned.a.x); - } else { - // near gimbal lock: denom ~ 0 => cos(pitch) ~ 0 (pitch ~ +/-90°) - // choose a stable convention: set roll to 0 and recover yaw from another pair - pitch = (cx > 0) ? -M_PI_2 : M_PI_2; // match existing sign convention - roll = 0.0f; - // use an alternate element to estimate yaw in singular case - yaw = atan2F(-rot_body_to_ned.a.y, rot_body_to_ned.b.y); + rot_body_to_ned.to_euler(&roll, &pitch, &yaw); + + // compensate for going beyond +/- 90 degree in pitch + if(ahrs.pitch_sensor<=0){ + if(pitch > 0.0f) { + pitch = 3.14f - pitch; + } + else { + pitch = -(pitch)- 3.14f; + } } roll_sensor = degrees(roll) * 100; From b9be9bcfbe121e9e138b94f7abe9e59ff43dc5c1 Mon Sep 17 00:00:00 2001 From: Mayank Joneja Date: Thu, 8 Jan 2026 11:15:03 +0530 Subject: [PATCH 05/31] Add rot matrix based check for nose down belly up/down --- libraries/AP_AHRS/AP_AHRS_View.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_View.cpp b/libraries/AP_AHRS/AP_AHRS_View.cpp index 09fc3f1d42559a..fcb0bda1e5e49a 100644 --- a/libraries/AP_AHRS/AP_AHRS_View.cpp +++ b/libraries/AP_AHRS/AP_AHRS_View.cpp @@ -70,13 +70,22 @@ void AP_AHRS_View::update() rot_body_to_ned.to_euler(&roll, &pitch, &yaw); + 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; + + // compensate for going beyond +/- 90 degree in pitch - if(ahrs.pitch_sensor<=0){ - if(pitch > 0.0f) { - pitch = 3.14f - pitch; + if(z_ned.z <= 0 ){ + // Nose down below horizon + if(x_ned.z > 0.0f) { + // Belly down + pitch = M_PI - pitch; } else { - pitch = -(pitch)- 3.14f; + // Belly up + pitch = -M_PI - pitch; } } From be8846e936f0b8677463cd6eb69d5c9337624686 Mon Sep 17 00:00:00 2001 From: Mayank Joneja Date: Thu, 8 Jan 2026 11:36:56 +0530 Subject: [PATCH 06/31] Swap pitch corrections in ahrs view --- libraries/AP_AHRS/AP_AHRS_View.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_View.cpp b/libraries/AP_AHRS/AP_AHRS_View.cpp index fcb0bda1e5e49a..16833e6a74dab6 100644 --- a/libraries/AP_AHRS/AP_AHRS_View.cpp +++ b/libraries/AP_AHRS/AP_AHRS_View.cpp @@ -81,11 +81,11 @@ void AP_AHRS_View::update() // Nose down below horizon if(x_ned.z > 0.0f) { // Belly down - pitch = M_PI - pitch; + pitch = -M_PI - pitch; } else { // Belly up - pitch = -M_PI - pitch; + pitch = M_PI - pitch; } } From 46b98b4b9f6495348d534899068d02e4ba634829 Mon Sep 17 00:00:00 2001 From: Mayank Joneja Date: Fri, 9 Jan 2026 12:28:01 +0530 Subject: [PATCH 07/31] Add logging for ahrs view compensation --- ArduPlane/Log.cpp | 12 ++++++++++ .../AC_AttitudeControl/AC_AttitudeControl.cpp | 1 + .../AC_AttitudeControl/AC_AttitudeControl.h | 9 ++++++++ libraries/AP_AHRS/AP_AHRS_Logging.cpp | 15 ++++++++++++ libraries/AP_AHRS/AP_AHRS_View.cpp | 10 ++++---- libraries/AP_AHRS/AP_AHRS_View.h | 6 +++-- libraries/AP_AHRS/LogStructure.h | 23 +++++++++++++++++-- 7 files changed, 67 insertions(+), 9 deletions(-) diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index 2df80fbbc1415c..5329196f25dffe 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/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..9d4c20171aa572 100644 --- a/libraries/AP_AHRS/AP_AHRS_Logging.cpp +++ b/libraries/AP_AHRS/AP_AHRS_Logging.cpp @@ -143,6 +143,21 @@ 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_old : pitch, + pitch_compensated : pitch_compensated, + z_ned_z : z_ned.z, + x_ned_z : x_ned.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..9b71fbe9567de0 100644 --- a/libraries/AP_AHRS/AP_AHRS_View.cpp +++ b/libraries/AP_AHRS/AP_AHRS_View.cpp @@ -72,25 +72,25 @@ void AP_AHRS_View::update() 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 if(x_ned.z > 0.0f) { // Belly down - pitch = -M_PI - pitch; + pitch_compensated = -M_PI - pitch; } else { // Belly up - pitch = M_PI - pitch; + pitch_compensated = M_PI - pitch; } } roll_sensor = degrees(roll) * 100; - pitch_sensor = degrees(pitch) * 100; + pitch_sensor = degrees(pitch_compensated) * 100; yaw_sensor = degrees(yaw) * 100; if (yaw_sensor < 0) { yaw_sensor += 36000; diff --git a/libraries/AP_AHRS/AP_AHRS_View.h b/libraries/AP_AHRS/AP_AHRS_View.h index 1ad748062f5023..6f4e9dbb0830fc 100644 --- a/libraries/AP_AHRS/AP_AHRS_View.h +++ b/libraries/AP_AHRS/AP_AHRS_View.h @@ -174,17 +174,19 @@ 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_Rate( const AP_Motors &motors, const AC_AttitudeControl &attitude_control, const AC_PosControl &pos_control) const; float roll; - float pitch; + float pitch, pitch_compensated; float yaw; int32_t roll_sensor; int32_t pitch_sensor; int32_t yaw_sensor; + Vector3f z_ned, x_ned; // 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..ceb6d45407bb17 100644 --- a/libraries/AP_AHRS/LogStructure.h +++ b/libraries/AP_AHRS/LogStructure.h @@ -9,7 +9,8 @@ LOG_ORGN_MSG, \ LOG_POS_MSG, \ LOG_RATE_MSG, \ - LOG_ATSC_MSG + LOG_ATSC_MSG, \ + LOG_ATT_PIT_COMP_MSG // @LoggerMessage: AHR2 // @Description: Backup AHRS data @@ -74,6 +75,22 @@ struct PACKED log_Attitude { uint8_t active; }; +// @LoggerMessage: ATPC +// @Description: Canonical vehicle attitude pitch compensation +// @Field: TimeUS: Time since system startup +// @Field: Pitch: vehicle pitch +// @Field: PitchComp: vehicle pitch post compensation for horizon crossing +// @Field: ZNedZ: Body Z vector's Z component in NED frame +// @Field: XNedZ: Body X vector's Z component in NED frame +struct PACKED log_AttitudeViewCompensation { + LOG_PACKET_HEADER; + uint64_t time_us; + float pitch_old; + float pitch_compensated; + float z_ned_z; + float x_ned_z; +}; + // @LoggerMessage: ORGN // @Description: Vehicle navigation origin or other notable position // @Field: TimeUS: Time since system startup @@ -209,5 +226,7 @@ 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", "Qffff", "TimeUS,AttPOld,AttPNew,ZNedZ,XNedZ", "sdd--", "F0000" }, From 439dcd3e6ad965e0376924aee5fcb651c7dde502 Mon Sep 17 00:00:00 2001 From: Mayank Joneja Date: Fri, 9 Jan 2026 13:04:25 +0530 Subject: [PATCH 08/31] Fix logging of pitch before and after compensation --- libraries/AP_AHRS/AP_AHRS_Logging.cpp | 4 ++-- libraries/AP_AHRS/AP_AHRS_View.cpp | 8 +++++--- libraries/AP_AHRS/AP_AHRS_View.h | 2 +- libraries/AP_AHRS/LogStructure.h | 10 +++++----- 4 files changed, 13 insertions(+), 11 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_Logging.cpp b/libraries/AP_AHRS/AP_AHRS_Logging.cpp index 9d4c20171aa572..052fb3001a4204 100644 --- a/libraries/AP_AHRS/AP_AHRS_Logging.cpp +++ b/libraries/AP_AHRS/AP_AHRS_Logging.cpp @@ -149,8 +149,8 @@ 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_old : pitch, - pitch_compensated : pitch_compensated, + 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, }; diff --git a/libraries/AP_AHRS/AP_AHRS_View.cpp b/libraries/AP_AHRS/AP_AHRS_View.cpp index 9b71fbe9567de0..9e48c9c66429d3 100644 --- a/libraries/AP_AHRS/AP_AHRS_View.cpp +++ b/libraries/AP_AHRS/AP_AHRS_View.cpp @@ -70,6 +70,8 @@ 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}; @@ -81,16 +83,16 @@ void AP_AHRS_View::update() // Nose down below horizon if(x_ned.z > 0.0f) { // Belly down - pitch_compensated = -M_PI - pitch; + pitch = -M_PI - pitch; } else { // Belly up - pitch_compensated = M_PI - pitch; + pitch = M_PI - pitch; } } roll_sensor = degrees(roll) * 100; - pitch_sensor = degrees(pitch_compensated) * 100; + pitch_sensor = degrees(pitch) * 100; yaw_sensor = degrees(yaw) * 100; if (yaw_sensor < 0) { yaw_sensor += 36000; diff --git a/libraries/AP_AHRS/AP_AHRS_View.h b/libraries/AP_AHRS/AP_AHRS_View.h index 6f4e9dbb0830fc..c10c2f17946289 100644 --- a/libraries/AP_AHRS/AP_AHRS_View.h +++ b/libraries/AP_AHRS/AP_AHRS_View.h @@ -180,7 +180,7 @@ class AP_AHRS_View const AC_PosControl &pos_control) const; float roll; - float pitch, pitch_compensated; + float pitch, pitch_raw; float yaw; int32_t roll_sensor; int32_t pitch_sensor; diff --git a/libraries/AP_AHRS/LogStructure.h b/libraries/AP_AHRS/LogStructure.h index ceb6d45407bb17..9f0f0e83adb011 100644 --- a/libraries/AP_AHRS/LogStructure.h +++ b/libraries/AP_AHRS/LogStructure.h @@ -85,10 +85,10 @@ struct PACKED log_Attitude { struct PACKED log_AttitudeViewCompensation { LOG_PACKET_HEADER; uint64_t time_us; - float pitch_old; - float pitch_compensated; - float z_ned_z; - float x_ned_z; + int16_t pitch; + int16_t pitch_raw; + float z_ned_z; + float x_ned_z; }; // @LoggerMessage: ORGN @@ -228,5 +228,5 @@ struct PACKED log_ATSC { { LOG_VIDEO_STABILISATION_MSG, sizeof(log_Video_Stabilisation), \ "VSTB", "Qffffffffff", "TimeUS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,Q1,Q2,Q3,Q4", "sEEEooo----", "F0000000000" }, \ { LOG_ATT_PIT_COMP_MSG, sizeof(log_AttitudeViewCompensation), \ - "ATPC", "Qffff", "TimeUS,AttPOld,AttPNew,ZNedZ,XNedZ", "sdd--", "F0000" }, + "ATPC", "Qccff", "TimeUS,AttPNew,AttPOld,ZNedZ,XNedZ", "sdd--", "FBB00" }, From b42218ba467d008c5eb322fbdb462204f6d95118 Mon Sep 17 00:00:00 2001 From: Mayank Joneja Date: Fri, 9 Jan 2026 16:02:01 +0530 Subject: [PATCH 09/31] Add logging for att target axis angle --- ArduPlane/Log.cpp | 2 +- libraries/AP_AHRS/AP_AHRS_Logging.cpp | 12 ++++++++++++ libraries/AP_AHRS/AP_AHRS_View.h | 3 +++ libraries/AP_AHRS/LogStructure.h | 22 ++++++++++++++++++++-- 4 files changed, 36 insertions(+), 3 deletions(-) diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index 5329196f25dffe..b5cb0d70fe163a 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -27,7 +27,7 @@ void Plane::Log_Write_Attitude(void) 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_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); diff --git a/libraries/AP_AHRS/AP_AHRS_Logging.cpp b/libraries/AP_AHRS/AP_AHRS_Logging.cpp index 052fb3001a4204..c6ad1762988162 100644 --- a/libraries/AP_AHRS/AP_AHRS_Logging.cpp +++ b/libraries/AP_AHRS/AP_AHRS_Logging.cpp @@ -157,6 +157,18 @@ void AP_AHRS_View::Write_AttitudePitchCompensation() const 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 a rate packet void AP_AHRS_View::Write_Rate(const AP_Motors &motors, const AC_AttitudeControl &attitude_control, diff --git a/libraries/AP_AHRS/AP_AHRS_View.h b/libraries/AP_AHRS/AP_AHRS_View.h index c10c2f17946289..46cfcb4cb2a2d0 100644 --- a/libraries/AP_AHRS/AP_AHRS_View.h +++ b/libraries/AP_AHRS/AP_AHRS_View.h @@ -176,6 +176,9 @@ class AP_AHRS_View // Logging Functions 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_Rate( const AP_Motors &motors, const AC_AttitudeControl &attitude_control, const AC_PosControl &pos_control) const; diff --git a/libraries/AP_AHRS/LogStructure.h b/libraries/AP_AHRS/LogStructure.h index 9f0f0e83adb011..2804ce71ac2f33 100644 --- a/libraries/AP_AHRS/LogStructure.h +++ b/libraries/AP_AHRS/LogStructure.h @@ -10,7 +10,9 @@ LOG_POS_MSG, \ LOG_RATE_MSG, \ LOG_ATSC_MSG, \ - LOG_ATT_PIT_COMP_MSG + LOG_ATT_PIT_COMP_MSG, \ + LOG_ATT_TAR_AA_MSG + // @LoggerMessage: AHR2 // @Description: Backup AHRS data @@ -91,6 +93,20 @@ struct PACKED log_AttitudeViewCompensation { float x_ned_z; }; +// @LoggerMessage: ATAT +// @Description: Canonical vehicle attitude target axis angle representation of body to NED quat +// @Field: TimeUS: Time since system startup +// @Field: TargetX: X component of axis angle vector +// @Field: TargetY: Y component of axis angle vector +// @Field: TargetZ: 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: ORGN // @Description: Vehicle navigation origin or other notable position // @Field: TimeUS: Time since system startup @@ -228,5 +244,7 @@ struct PACKED log_ATSC { { LOG_VIDEO_STABILISATION_MSG, sizeof(log_Video_Stabilisation), \ "VSTB", "Qffffffffff", "TimeUS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,Q1,Q2,Q3,Q4", "sEEEooo----", "F0000000000" }, \ { LOG_ATT_PIT_COMP_MSG, sizeof(log_AttitudeViewCompensation), \ - "ATPC", "Qccff", "TimeUS,AttPNew,AttPOld,ZNedZ,XNedZ", "sdd--", "FBB00" }, + "ATPC", "Qccff", "TimeUS,AttPNew,AttPOld,ZNedZ,XNedZ", "sdd--", "FBB00" }, \ + { LOG_ATT_TAR_AA_MSG, sizeof(log_AttitudeTarAxisAng), \ + "ATAT", "Qfff", "TimeUS,TarX,TarY,TarZ", "s---", "F000" }, \ From de2de56b5f1c2d4e0e9f50187542379c20f1d6cf Mon Sep 17 00:00:00 2001 From: Mayank Joneja Date: Fri, 9 Jan 2026 16:58:07 +0530 Subject: [PATCH 10/31] Add logging of att body and error quat as axis angle --- ArduPlane/Log.cpp | 4 +-- libraries/AP_AHRS/AP_AHRS_Logging.cpp | 25 ++++++++++++++++ libraries/AP_AHRS/AP_AHRS_View.h | 2 ++ libraries/AP_AHRS/LogStructure.h | 42 ++++++++++++++++++++++++--- 4 files changed, 67 insertions(+), 6 deletions(-) diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index b5cb0d70fe163a..8da8ecc0cd4008 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -28,8 +28,8 @@ void Plane::Log_Write_Attitude(void) 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_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 diff --git a/libraries/AP_AHRS/AP_AHRS_Logging.cpp b/libraries/AP_AHRS/AP_AHRS_Logging.cpp index c6ad1762988162..600e5640c4fb88 100644 --- a/libraries/AP_AHRS/AP_AHRS_Logging.cpp +++ b/libraries/AP_AHRS/AP_AHRS_Logging.cpp @@ -170,6 +170,31 @@ void AP_AHRS_View::Write_Att_Tar_Axis_Ang(Vector3f att_tar_axis_ang) const 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.h b/libraries/AP_AHRS/AP_AHRS_View.h index 46cfcb4cb2a2d0..8a59b97d83bcc4 100644 --- a/libraries/AP_AHRS/AP_AHRS_View.h +++ b/libraries/AP_AHRS/AP_AHRS_View.h @@ -178,6 +178,8 @@ class AP_AHRS_View 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; diff --git a/libraries/AP_AHRS/LogStructure.h b/libraries/AP_AHRS/LogStructure.h index 2804ce71ac2f33..7fb077b81e1b48 100644 --- a/libraries/AP_AHRS/LogStructure.h +++ b/libraries/AP_AHRS/LogStructure.h @@ -11,7 +11,9 @@ LOG_RATE_MSG, \ LOG_ATSC_MSG, \ LOG_ATT_PIT_COMP_MSG, \ - LOG_ATT_TAR_AA_MSG + LOG_ATT_TAR_AA_MSG, \ + LOG_ATT_BOD_AA_MSG, \ + LOG_ATT_ERR_AA_MSG // @LoggerMessage: AHR2 @@ -96,9 +98,9 @@ struct PACKED log_AttitudeViewCompensation { // @LoggerMessage: ATAT // @Description: Canonical vehicle attitude target axis angle representation of body to NED quat // @Field: TimeUS: Time since system startup -// @Field: TargetX: X component of axis angle vector -// @Field: TargetY: Y component of axis angle vector -// @Field: TargetZ: Z component of axis angle vector +// @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; @@ -107,6 +109,34 @@ struct PACKED log_AttitudeTarAxisAng { 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 @@ -247,4 +277,8 @@ struct PACKED log_ATSC { "ATPC", "Qccff", "TimeUS,AttPNew,AttPOld,ZNedZ,XNedZ", "sdd--", "FBB00" }, \ { 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" }, From 6a0e179f7cc1f8d52c6fa4fbbd7a42f57e0df415 Mon Sep 17 00:00:00 2001 From: Mayank Joneja Date: Fri, 9 Jan 2026 17:33:08 +0530 Subject: [PATCH 11/31] Add flags for belly up/down and nose down in logs --- libraries/AP_AHRS/AP_AHRS_Logging.cpp | 2 ++ libraries/AP_AHRS/AP_AHRS_View.cpp | 6 ++++++ libraries/AP_AHRS/AP_AHRS_View.h | 1 + libraries/AP_AHRS/LogStructure.h | 6 +++++- 4 files changed, 14 insertions(+), 1 deletion(-) diff --git a/libraries/AP_AHRS/AP_AHRS_Logging.cpp b/libraries/AP_AHRS/AP_AHRS_Logging.cpp index 600e5640c4fb88..f20bb8383e011b 100644 --- a/libraries/AP_AHRS/AP_AHRS_Logging.cpp +++ b/libraries/AP_AHRS/AP_AHRS_Logging.cpp @@ -153,6 +153,8 @@ void AP_AHRS_View::Write_AttitudePitchCompensation() const 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)); } diff --git a/libraries/AP_AHRS/AP_AHRS_View.cpp b/libraries/AP_AHRS/AP_AHRS_View.cpp index 9e48c9c66429d3..8fc51e1dd9d5a7 100644 --- a/libraries/AP_AHRS/AP_AHRS_View.cpp +++ b/libraries/AP_AHRS/AP_AHRS_View.cpp @@ -81,14 +81,20 @@ void AP_AHRS_View::update() // 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 8a59b97d83bcc4..50347705627668 100644 --- a/libraries/AP_AHRS/AP_AHRS_View.h +++ b/libraries/AP_AHRS/AP_AHRS_View.h @@ -192,6 +192,7 @@ class AP_AHRS_View 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 7fb077b81e1b48..9dd5275099a0b2 100644 --- a/libraries/AP_AHRS/LogStructure.h +++ b/libraries/AP_AHRS/LogStructure.h @@ -86,6 +86,8 @@ struct PACKED log_Attitude { // @Field: PitchComp: vehicle pitch post compensation for horizon crossing // @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; @@ -93,6 +95,8 @@ struct PACKED log_AttitudeViewCompensation { int16_t pitch_raw; float z_ned_z; float x_ned_z; + float is_nose_down; + float is_belly_up; }; // @LoggerMessage: ATAT @@ -274,7 +278,7 @@ struct PACKED log_ATSC { { LOG_VIDEO_STABILISATION_MSG, sizeof(log_Video_Stabilisation), \ "VSTB", "Qffffffffff", "TimeUS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,Q1,Q2,Q3,Q4", "sEEEooo----", "F0000000000" }, \ { LOG_ATT_PIT_COMP_MSG, sizeof(log_AttitudeViewCompensation), \ - "ATPC", "Qccff", "TimeUS,AttPNew,AttPOld,ZNedZ,XNedZ", "sdd--", "FBB00" }, \ + "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), \ From 51850627b0121d251726b55970b2cf76af41a27e Mon Sep 17 00:00:00 2001 From: Mayank Joneja Date: Fri, 9 Jan 2026 17:33:38 +0530 Subject: [PATCH 12/31] Update version to rc6 --- ArduPlane/version.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduPlane/version.h b/ArduPlane/version.h index 975fc439de1a56..4da7a1734f499c 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 - rc5" +#define THISFIRMWARE "Airbound ArduPlane V4.5.7.2 - rc6" // the following line is parsed by the autotest scripts #define FIRMWARE_VERSION 4,5,7,FIRMWARE_VERSION_TYPE_DEV From cd141fd6a10c8cc11d32592992fb7c59d6d14f2c Mon Sep 17 00:00:00 2001 From: Mayank Joneja Date: Sat, 10 Jan 2026 12:00:41 +0530 Subject: [PATCH 13/31] Change version to 4.5.7.3 rc1 --- ArduPlane/version.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduPlane/version.h b/ArduPlane/version.h index 4da7a1734f499c..8a655b0cb5a09c 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 - rc6" +#define THISFIRMWARE "Airbound ArduPlane V4.5.7.3 - rc1" // the following line is parsed by the autotest scripts #define FIRMWARE_VERSION 4,5,7,FIRMWARE_VERSION_TYPE_DEV From 717e0c5412558849f81e7171ab21f04a77c1f1bc Mon Sep 17 00:00:00 2001 From: Mayank Joneja Date: Thu, 15 Jan 2026 12:36:20 +0530 Subject: [PATCH 14/31] Change mode check for landing phase for doors script --- .../scripts/ab_door_auto_control.lua | 20 ++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) 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..12486a1b141de3 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 @@ -241,13 +241,23 @@ function get_tailsitter_flight_state() end function is_in_landing_phase(current_mode, flight_state) - local MODES = { qland = 20, land = 21, rtl = 11, auto = 10 } + local QMODES = { + [11] = true, -- RTL + [17] = true, -- QSTABILIZE + [18] = true, -- QHOVER + [19] = true, -- QLOITER + [20] = true, -- QLAND + [21] = true, -- QRTL + [22] = true, -- QAUTOTUNE + [23] = true, -- QACRO + } + + local MODES = { auto = 10 } -- other valid modes which need additional checks if not current_mode or not flight_state 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 if current_mode == MODES.auto then From 8f5952c8db104c8cc8bad683d02c88b3aa2c1b50 Mon Sep 17 00:00:00 2001 From: Mayank Joneja Date: Fri, 16 Jan 2026 11:12:45 +0530 Subject: [PATCH 15/31] Check valid landing mission commands --- .../scripts/ab_door_auto_control.lua | 41 +++++++++++-------- 1 file changed, 24 insertions(+), 17 deletions(-) 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 12486a1b141de3..b08fdeea44b86d 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,8 +11,25 @@ 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 @@ -240,20 +257,9 @@ function get_tailsitter_flight_state() else return "TRANSITIONING" end end -function is_in_landing_phase(current_mode, flight_state) - local QMODES = { - [11] = true, -- RTL - [17] = true, -- QSTABILIZE - [18] = true, -- QHOVER - [19] = true, -- QLOITER - [20] = true, -- QLAND - [21] = true, -- QRTL - [22] = true, -- QAUTOTUNE - [23] = true, -- QACRO - } - +function is_in_landing_phase(current_mode) local MODES = { auto = 10 } -- other valid modes which need additional checks - if not current_mode or not flight_state then return false end + if not current_mode then return false end -- Return true for all q modes directly, regardless of pitch if QMODES[current_mode] then @@ -262,7 +268,8 @@ function is_in_landing_phase(current_mode, flight_state) 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 @@ -282,7 +289,7 @@ 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 is_in_landing_phase(current_mode) then if state.doors_are_commanded_closed then log_message(MAV_SEVERITY.WARNING, "Landing confirmed - opening doors") open_all_doors() From 57a78d743475c326d8fce5820dddead6b1f5b98e Mon Sep 17 00:00:00 2001 From: Mayank Joneja Date: Wed, 21 Jan 2026 11:55:16 +0530 Subject: [PATCH 16/31] Add check for vtol land descent from quadplane --- .../hwdef/Pixhawk6C-bdshot/scripts/ab_door_auto_control.lua | 6 ++++++ 1 file changed, 6 insertions(+) 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 b08fdeea44b86d..f0bdb7f96fcfa5 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 @@ -266,6 +266,7 @@ function is_in_landing_phase(current_mode) 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_CMDS[nav_cmd] then @@ -274,6 +275,11 @@ function is_in_landing_phase(current_mode) end end + -- Check VTOL descent flag + if quadplane and quadplane:in_vtol_land_descent() then + return true + end + return false end From 93bb8d27ca6969a92ba7bfe829dc71fad698cd8e Mon Sep 17 00:00:00 2001 From: Mayank Joneja Date: Wed, 21 Jan 2026 12:02:49 +0530 Subject: [PATCH 17/31] Force doors open under takeoff alt_trig_m and don't check for transitions during takeoff --- .../hwdef/Pixhawk6C-bdshot/scripts/ab_door_auto_control.lua | 4 ++++ 1 file changed, 4 insertions(+) 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 f0bdb7f96fcfa5..bbcc4c73d882cf 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 @@ -309,7 +309,11 @@ 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 + return end else if state.was_in_fw_state and is_currently_in_vtol then From 8d25f52ef6915b7ecbcdc891faf6ebf66063e68b Mon Sep 17 00:00:00 2001 From: Mayank Joneja Date: Wed, 21 Jan 2026 12:29:09 +0530 Subject: [PATCH 18/31] Add a landing latch flag to only trigger doors basis modes --- .../scripts/ab_door_auto_control.lua | 53 +++++++++++++------ 1 file changed, 36 insertions(+), 17 deletions(-) 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 bbcc4c73d882cf..d3afce7774404c 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 @@ -96,6 +96,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 = {} } @@ -295,13 +296,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) 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 @@ -313,20 +309,41 @@ function run_auto_mode() -- Force doors open under alt_trig_m open_all_doors() end - return - 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() + else + -- Latch is NOT set. + -- Check if we are now vertical (Pitch > 75). + if is_currently_in_vtol then + state.landing_latch = true + open_all_doors() + end + 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 @@ -395,6 +412,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() @@ -402,6 +420,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 From 3642cad05da0aab30b135e10e713068f41690db1 Mon Sep 17 00:00:00 2001 From: Mayank Joneja Date: Wed, 21 Jan 2026 14:44:01 +0530 Subject: [PATCH 19/31] Initialize servo pwm open close based on specific defaults --- .../Pixhawk6C-bdshot/scripts/ab_door_auto_control.lua | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) 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 d3afce7774404c..e22a6a7c593d54 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 @@ -71,10 +71,13 @@ 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 From 07200d7cd82b45107560e22dfa162ec944adfa65 Mon Sep 17 00:00:00 2001 From: Mayank Joneja Date: Wed, 21 Jan 2026 14:48:19 +0530 Subject: [PATCH 20/31] Update version to 4.5.7.2 hf1 --- ArduPlane/version.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduPlane/version.h b/ArduPlane/version.h index c595533f6c17ea..2d9561132ad991 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" // the following line is parsed by the autotest scripts #define FIRMWARE_VERSION 4,5,7,FIRMWARE_VERSION_TYPE_DEV From a81a5c6157cd6813cc45230924feb53d242baf6d Mon Sep 17 00:00:00 2001 From: Mayank Joneja Date: Wed, 21 Jan 2026 15:00:02 +0530 Subject: [PATCH 21/31] Update version to v4.5.7.2 - hf1 --- ArduPlane/version.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduPlane/version.h b/ArduPlane/version.h index 8a655b0cb5a09c..2d9561132ad991 100644 --- a/ArduPlane/version.h +++ b/ArduPlane/version.h @@ -6,7 +6,7 @@ #include "ap_version.h" -#define THISFIRMWARE "Airbound ArduPlane V4.5.7.3 - rc1" +#define THISFIRMWARE "Airbound ArduPlane V4.5.7.2 - hf1" // the following line is parsed by the autotest scripts #define FIRMWARE_VERSION 4,5,7,FIRMWARE_VERSION_TYPE_DEV From 305ebb6d392cecdbad56936c21462118151051f0 Mon Sep 17 00:00:00 2001 From: Mayank Joneja Date: Wed, 21 Jan 2026 16:45:23 +0530 Subject: [PATCH 22/31] Fix vtol flight check logic basis latch --- .../Pixhawk6C-bdshot/scripts/ab_door_auto_control.lua | 7 ------- 1 file changed, 7 deletions(-) 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 e22a6a7c593d54..01600458efda13 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 @@ -331,13 +331,6 @@ function run_auto_mode() -- We are latched OPEN. -- Stays TRUE even if pitch goes to 0 (Fix for instability safety). open_all_doors() - else - -- Latch is NOT set. - -- Check if we are now vertical (Pitch > 75). - if is_currently_in_vtol then - state.landing_latch = true - open_all_doors() - end end -- latch checking else -- Non-VTOL Modes (FW, FBWA, MANUAL, etc.) From 7615a9012bde61d12c040a89a45826a332c0cc4f Mon Sep 17 00:00:00 2001 From: Mayank Joneja Date: Wed, 21 Jan 2026 17:08:17 +0530 Subject: [PATCH 23/31] Add global override param for doors auto logic --- .../scripts/ab_door_auto_control.lua | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) 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 01600458efda13..0c1987d9d8e52b 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 @@ -56,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 --]] @@ -84,6 +87,11 @@ end --[[ @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 @@ -110,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 = {} @@ -141,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 @@ -347,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 @@ -358,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 From 102f425618397d2e039ef0a7f9a0d7273f981b41 Mon Sep 17 00:00:00 2001 From: Mayank Joneja Date: Wed, 21 Jan 2026 17:48:15 +0530 Subject: [PATCH 24/31] Fix num params for door script --- .../hwdef/Pixhawk6C-bdshot/scripts/ab_door_auto_control.lua | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 0c1987d9d8e52b..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 @@ -36,7 +36,7 @@ local NAV_CMDS = { -- ############################################################################### 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 = { From e30dab2d6b7a3139e5db3864dc2debb9538bdb7b Mon Sep 17 00:00:00 2001 From: Mayank Joneja Date: Thu, 22 Jan 2026 17:20:42 +0530 Subject: [PATCH 25/31] Fix docstring for ATPC logging --- libraries/AP_AHRS/LogStructure.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_AHRS/LogStructure.h b/libraries/AP_AHRS/LogStructure.h index 9dd5275099a0b2..8cd0fe00274578 100644 --- a/libraries/AP_AHRS/LogStructure.h +++ b/libraries/AP_AHRS/LogStructure.h @@ -82,8 +82,8 @@ struct PACKED log_Attitude { // @LoggerMessage: ATPC // @Description: Canonical vehicle attitude pitch compensation // @Field: TimeUS: Time since system startup -// @Field: Pitch: vehicle pitch -// @Field: PitchComp: vehicle pitch post compensation for horizon crossing +// @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 @@ -95,8 +95,8 @@ struct PACKED log_AttitudeViewCompensation { int16_t pitch_raw; float z_ned_z; float x_ned_z; - float is_nose_down; - float is_belly_up; + float is_nose_down; + float is_belly_up; }; // @LoggerMessage: ATAT From befdd50e5da8dfd91263a824b82a448082376a77 Mon Sep 17 00:00:00 2001 From: Tarun Date: Wed, 28 Jan 2026 11:21:29 +0530 Subject: [PATCH 26/31] Improve tailsitter transition by adding heading alignment, timeout safety --- ArduPlane/tailsitter.cpp | 82 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 82 insertions(+) diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index de33fcd130c392..3e2b67931c0d61 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -903,6 +903,88 @@ void Tailsitter_Transition::update() switch (transition_state) { case TRANSITION_ANGLE_WAIT_FW: { + + const uint32_t now_ = AP_HAL::millis(); + + // ----------------------------------------------------------- + // 1. INITIALIZATION + // ----------------------------------------------------------- + // We set motors to unlimited and start the timer immediately. + // This is required so the alignment logic below has a valid start time. + quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); + + // ----------------------------------------------------------- + // 2. NEW: Robust Pre-Transition Heading Alignment + // ----------------------------------------------------------- + if (quadplane.tailsitter.enabled()) { + + // Constants for Alignment + const int32_t ALIGN_TOLERANCE_CD = 500; // 5.0 degrees + const uint32_t ALIGN_TIMEOUT_MS = 5000; // 5.0 seconds limit + const float ALIGN_YAW_GAIN = 2.0f; // Yaw P-gain + + // Safety Check: Only align in Auto/Guided and if Nav Controller is healthy + // bool should_align = false; + int32_t target_bearing_cd = 0; + + if ((plane.control_mode == &plane.mode_auto || plane.control_mode == &plane.mode_guided) && + plane.nav_controller != nullptr) { + + target_bearing_cd = plane.nav_controller->target_bearing_cd(); + + // Calculate Heading Error + const int32_t current_yaw_cd = quadplane.ahrs.yaw_sensor; + const int32_t error_cd = wrap_180_cd(target_bearing_cd - current_yaw_cd); + + // Align if error is large AND we haven't timed out (using transition_start_ms) + if (abs(error_cd) > ALIGN_TOLERANCE_CD) { + if (now_ - fw_transition_start_ms < ALIGN_TIMEOUT_MS) { + + // -- A. Log Status (Throttled) -- + if ((now_ - fw_transition_start_ms) % 1000 < 50) { + gcs().send_text(MAV_SEVERITY_INFO, "Aligning: Err %.1f deg", error_cd * 0.01f); + } + + // -- B. Calculate Control Targets -- + // Run Position Controllers (Z + XY) to get wind-fighting targets + quadplane.pos_control->update_z_controller(); + quadplane.pos_control->update_xy_controller(); + + float target_roll_cd = quadplane.pos_control->get_roll_cd(); + float target_pitch_cd = quadplane.pos_control->get_pitch_cd(); + + // Calculate Custom Yaw Rate (P-Controller) + float target_yaw_rate_cds = error_cd * ALIGN_YAW_GAIN; + + // Clamp rate to Pilot limits + const float max_rate_cds = quadplane.command_model_pilot.get_rate() * 100.0f; + target_yaw_rate_cds = constrain_float(target_yaw_rate_cds, -max_rate_cds, max_rate_cds); + + // -- C. Command Attitude -- + // Feed Roll/Pitch (Angle) and Yaw (Rate) to controller + quadplane.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw( + target_roll_cd, + target_pitch_cd, + target_yaw_rate_cds + ); + + // -- D. Output & Block -- + quadplane.motors_output(); + set_last_fw_pitch(); + + return; // CRITICAL: Stop here. Do not proceed until aligned. + } + else { + // Timeout has occurred. Log warning once and fall through. + static bool timeout_warned = false; + if (!timeout_warned) { + gcs().send_text(MAV_SEVERITY_WARNING, "Align Timeout: Proceeding"); + timeout_warned = true; + } + } + } + } + } if (tailsitter.transition_fw_complete()) { transition_state = TRANSITION_DONE; if (plane.arming.is_armed_and_safety_off()) { From aedaa74cc1453a2e9f08aad349979b7d8caff355 Mon Sep 17 00:00:00 2001 From: Tarun Date: Wed, 28 Jan 2026 18:53:51 +0530 Subject: [PATCH 27/31] Resolve belly flop and rough transition issues with improved tailsitter alignment and handover logic --- ArduPlane/tailsitter.cpp | 163 ++++++++++++++++++++++++--------------- 1 file changed, 102 insertions(+), 61 deletions(-) diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index 3e2b67931c0d61..68fb3f4ea56aa1 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -909,82 +909,123 @@ void Tailsitter_Transition::update() // ----------------------------------------------------------- // 1. INITIALIZATION // ----------------------------------------------------------- - // We set motors to unlimited and start the timer immediately. - // This is required so the alignment logic below has a valid start time. quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // ----------------------------------------------------------- - // 2. NEW: Robust Pre-Transition Heading Alignment + // 2. TAILSITTER HEADING ALIGNMENT & WAIT // ----------------------------------------------------------- if (quadplane.tailsitter.enabled()) { - // Constants for Alignment - const int32_t ALIGN_TOLERANCE_CD = 500; // 5.0 degrees - const uint32_t ALIGN_TIMEOUT_MS = 5000; // 5.0 seconds limit - const float ALIGN_YAW_GAIN = 2.0f; // Yaw P-gain + // Constants + const int32_t ALIGN_TOLERANCE_CD = 500; // 5.0 degrees + const float MAX_SPIN_RATE_DEG = 5.0f; // Max yaw rate allowed + const uint32_t ALIGN_PHASE_LIMIT_MS = 10000; // 10s Total Timeout + const uint32_t WAIT_DELAY_MS = 3000; // 3s Wait + const float ALIGN_YAW_GAIN = 2.0f; + + // 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_; - // Safety Check: Only align in Auto/Guided and if Nav Controller is healthy - // bool should_align = false; - int32_t target_bearing_cd = 0; + // 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 ((plane.control_mode == &plane.mode_auto || plane.control_mode == &plane.mode_guided) && - plane.nav_controller != nullptr) { - - target_bearing_cd = plane.nav_controller->target_bearing_cd(); + if (should_run_alignment) { - // Calculate Heading Error - const int32_t current_yaw_cd = quadplane.ahrs.yaw_sensor; - const int32_t error_cd = wrap_180_cd(target_bearing_cd - current_yaw_cd); - - // Align if error is large AND we haven't timed out (using transition_start_ms) - if (abs(error_cd) > ALIGN_TOLERANCE_CD) { - if (now_ - fw_transition_start_ms < ALIGN_TIMEOUT_MS) { - - // -- A. Log Status (Throttled) -- - if ((now_ - fw_transition_start_ms) % 1000 < 50) { - gcs().send_text(MAV_SEVERITY_INFO, "Aligning: Err %.1f deg", error_cd * 0.01f); - } + 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 + } - // -- B. Calculate Control Targets -- - // Run Position Controllers (Z + XY) to get wind-fighting targets - quadplane.pos_control->update_z_controller(); - quadplane.pos_control->update_xy_controller(); - - float target_roll_cd = quadplane.pos_control->get_roll_cd(); - float target_pitch_cd = quadplane.pos_control->get_pitch_cd(); - - // Calculate Custom Yaw Rate (P-Controller) - float target_yaw_rate_cds = error_cd * ALIGN_YAW_GAIN; - - // Clamp rate to Pilot limits - const float max_rate_cds = quadplane.command_model_pilot.get_rate() * 100.0f; - target_yaw_rate_cds = constrain_float(target_yaw_rate_cds, -max_rate_cds, max_rate_cds); - - // -- C. Command Attitude -- - // Feed Roll/Pitch (Angle) and Yaw (Rate) to controller - quadplane.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw( - target_roll_cd, - target_pitch_cd, - target_yaw_rate_cds - ); - - // -- D. Output & Block -- - quadplane.motors_output(); - set_last_fw_pitch(); - - return; // CRITICAL: Stop here. Do not proceed until aligned. - } - else { - // Timeout has occurred. Log warning once and fall through. - static bool timeout_warned = false; - if (!timeout_warned) { - gcs().send_text(MAV_SEVERITY_WARNING, "Align Timeout: Proceeding"); - timeout_warned = true; + // 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_; } + + // Control + quadplane.pos_control->update_z_controller(); + quadplane.pos_control->update_xy_controller(); + + float target_yaw_rate_cds = error_cd * ALIGN_YAW_GAIN; + float max_rate = quadplane.command_model_pilot.get_rate() * 100.0f; + target_yaw_rate_cds = constrain_float(target_yaw_rate_cds, -max_rate, max_rate); + + quadplane.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw( + quadplane.pos_control->get_roll_cd(), + quadplane.pos_control->get_pitch_cd(), + target_yaw_rate_cds + ); + + 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(); } } + + // 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()) { From ca709505650c76d20024e6da50259deb7bdb5bfd Mon Sep 17 00:00:00 2001 From: Tarun Date: Fri, 30 Jan 2026 18:33:53 +0530 Subject: [PATCH 28/31] Disable weathervane and lock climb rate during transition alignment --- ArduPlane/tailsitter.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index 68fb3f4ea56aa1..78389d6c6adae6 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -905,6 +905,14 @@ void Tailsitter_Transition::update() case TRANSITION_ANGLE_WAIT_FW: { const uint32_t now_ = AP_HAL::millis(); + + // --- NEW: DISABLE WEATHERVANE --- + // This prevents the weathervane library from fighting your custom + // yaw alignment logic or drifting the nose into the wind. + + if (quadplane.weathervane != nullptr) { + quadplane.weathervane->allow_weathervaning(false); + } // ----------------------------------------------------------- // 1. INITIALIZATION @@ -982,6 +990,8 @@ void Tailsitter_Transition::update() } 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(); From 644d922cfce2a3890fbbfc293aef4040b08a1ded Mon Sep 17 00:00:00 2001 From: Tarun Date: Wed, 4 Feb 2026 11:44:12 +0530 Subject: [PATCH 29/31] Implement internal yaw angle controller, replacing manual P-gain control --- ArduPlane/tailsitter.cpp | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index 78389d6c6adae6..e53dacef29acf7 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -928,8 +928,7 @@ void Tailsitter_Transition::update() const int32_t ALIGN_TOLERANCE_CD = 500; // 5.0 degrees const float MAX_SPIN_RATE_DEG = 5.0f; // Max yaw rate allowed const uint32_t ALIGN_PHASE_LIMIT_MS = 10000; // 10s Total Timeout - const uint32_t WAIT_DELAY_MS = 3000; // 3s Wait - const float ALIGN_YAW_GAIN = 2.0f; + const uint32_t WAIT_DELAY_MS = 2000; // 2s Wait // Static Variables (State Tracking) static uint32_t align_phase_start_ms = 0; @@ -996,15 +995,11 @@ void Tailsitter_Transition::update() // Control quadplane.pos_control->update_z_controller(); quadplane.pos_control->update_xy_controller(); - - float target_yaw_rate_cds = error_cd * ALIGN_YAW_GAIN; - float max_rate = quadplane.command_model_pilot.get_rate() * 100.0f; - target_yaw_rate_cds = constrain_float(target_yaw_rate_cds, -max_rate, max_rate); - - quadplane.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw( + quadplane.attitude_control->input_euler_angle_roll_pitch_yaw( quadplane.pos_control->get_roll_cd(), quadplane.pos_control->get_pitch_cd(), - target_yaw_rate_cds + (float)target_bearing_cd, + true ); quadplane.motors_output(); @@ -1019,7 +1014,7 @@ void Tailsitter_Transition::update() } 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; From bdc4ae98037cb5663df029757128375f2adaae79 Mon Sep 17 00:00:00 2001 From: Tarun Date: Wed, 4 Feb 2026 14:46:46 +0530 Subject: [PATCH 30/31] Fix weathervane issue and update version to Auto Yaw Align --- ArduPlane/tailsitter.cpp | 27 ++++++++++++++++----------- modules/littlefs | 1 + 2 files changed, 17 insertions(+), 11 deletions(-) create mode 160000 modules/littlefs diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index e53dacef29acf7..cc997d75c074af 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -905,15 +905,6 @@ void Tailsitter_Transition::update() case TRANSITION_ANGLE_WAIT_FW: { const uint32_t now_ = AP_HAL::millis(); - - // --- NEW: DISABLE WEATHERVANE --- - // This prevents the weathervane library from fighting your custom - // yaw alignment logic or drifting the nose into the wind. - - if (quadplane.weathervane != nullptr) { - quadplane.weathervane->allow_weathervaning(false); - } - // ----------------------------------------------------------- // 1. INITIALIZATION // ----------------------------------------------------------- @@ -925,8 +916,8 @@ void Tailsitter_Transition::update() if (quadplane.tailsitter.enabled()) { // Constants - const int32_t ALIGN_TOLERANCE_CD = 500; // 5.0 degrees - const float MAX_SPIN_RATE_DEG = 5.0f; // Max yaw rate allowed + 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 @@ -954,6 +945,10 @@ void Tailsitter_Transition::update() (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; @@ -1026,6 +1021,16 @@ void Tailsitter_Transition::update() 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); + } } } 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 From 96dd024ed69d26dd16a27086e259da8dc84ad270 Mon Sep 17 00:00:00 2001 From: Tarun Date: Wed, 4 Feb 2026 14:54:16 +0530 Subject: [PATCH 31/31] Add version name (Auto Yaw Align) --- ArduPlane/version.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduPlane/version.h b/ArduPlane/version.h index 2d9561132ad991..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 - hf1" +#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