iNavFlight / inav

INAV: Navigation-enabled flight control software
https://inavflight.github.io
GNU General Public License v3.0
3.08k stars 1.46k forks source link

Altitude hold not working properly. #10132

Open Lalbabu2001 opened 3 months ago

Lalbabu2001 commented 3 months ago

PLEASE MAKE SURE YOU READ AND UNDERSTAND THE SOCIAL MEDIA SUPPORT CHANNELS. QUESTIONS ABOUT FLASHING, CONFIGURING, PILOTING MAY BE CLOSED WITHOUT FURTHER INTERACTION.

Please double-check that nobody reported the issue before by using search in this bug tracker.

PLEASE DELETE THE TEXT ABOVE AFTER READING AND UNDERSTANDING IT


Current Behavior

I have been facing this issue since day 1 , after reading those wrost documentation about althold for inav I still don't know how altitude hold mode works , how baro , acc z weight distribution works anyway let's discuss the problem which is whenever I fly in altitude hold mode the quad maintains altitude well, when when I try to make adjustment,let's say raise throttle above 50% to gains height but undershoot(bounces back few meters down) & same behaviour in case of throttle down also( bounces back up few meters). My setup is 2212 920kv motor, 10 inch pro, stm32f411, mpu 6500 , bmp280 , 3s 5500mah. Howering throttle is about 1350 .

Steps to Reproduce

1.arm in altitude hold mode, 2.raise throttle above 50% to lift quad couple of meters in the air . 3.lower throttle to 50%(middle) to maintain that altitude

  1. As you lower to 50% the quad bounce back down couple of meters and then maintain the altitude.

Expected behavior

I also flew ardupilot and never had this issue.

Suggested solution(s)

Additional context


sensei-hacker commented 3 months ago

There could be a couple different relative settings. Please attach your diff so we can see what your settings are. Thanks.

Lalbabu2001 commented 3 months ago

There could be a couple different relative settings. Please attach your diff so we can see what your settings are. Thanks.

Here it is:

diff all

version

INAV/STM32F411CE 6.1.1 Oct 7 2023 / 00:35:33 (GITDIR-N)

GCC-10.2.1 20201103 (release)

start the command batch

batch start

reset configuration to default settings

defaults noreboot

resources

Mixer: motor mixer

mmix reset

mmix 0 1.000 -1.000 1.000 -1.000 mmix 1 1.000 -1.000 -1.000 1.000 mmix 2 1.000 1.000 1.000 1.000 mmix 3 1.000 1.000 -1.000 -1.000

Mixer: servo mixer

smix reset

smix 0 1 12 350 0 -1 smix 1 1 10 100 0 -1

Outputs [servo]

servo 1 1300 2000 1500 -100 servo 2 1000 2000 1500 -100

safehome

features

feature -TELEMETRY feature -BLACKBOX feature -AIRMODE feature LED_STRIP feature PWM_OUTPUT_ENABLE

beeper

blackbox

blackbox NAV_ACC blackbox NAV_POS blackbox NAV_PID blackbox MAG blackbox ACC blackbox ATTI blackbox RC_DATA blackbox RC_COMMAND blackbox MOTORS blackbox GYRO_RAW blackbox PEAKS_R blackbox PEAKS_P blackbox PEAKS_Y

Receiver: Channel map

map TAER

Ports

serial 0 2 115200 115200 0 115200

LEDs

led 0 8,7::L:2

LED color

LED mode_color

Modes [aux]

aux 0 0 0 1725 2075 aux 1 1 0 925 2050 aux 2 3 0 950 2050

Adjustments [adjrange]

Receiver rxrange

temp_sensor

Mission Control Waypoints [wp]

wp 0 invalid

OSD [osd_layout]

Programming: logic

Programming: global variables

Programming: PID controllers

master

set looptime = 500 set gyro_main_lpf_hz = 40 set gyro_main_lpf_type = PT1 set dynamic_gyro_notch_q = 250 set dynamic_gyro_notch_min_hz = 60 set dynamic_gyro_notch_mode = 3D set setpoint_kalman_q = 200 set gyro_zero_x = 126 set gyro_zero_y = 166 set gyro_zero_z = 6 set ins_gravity_cmss = 977.204 set acc_hardware = MPU9250 set acc_lpf_hz = 5 set acczero_x = 8 set acczero_y = -25 set acczero_z = -44 set accgain_x = 4100 set accgain_y = 4092 set accgain_z = 4000 set align_mag = CW180 set mag_hardware = HMC5883 set magzero_x = 425 set magzero_y = 34 set magzero_z = 6 set maggain_x = 607 set maggain_y = 605 set maggain_z = 565 set baro_hardware = BMP280 set rc_filter_auto = OFF set blackbox_rate_denom = 2 set motor_pwm_protocol = STANDARD set failsafe_delay = 30 set failsafe_procedure = DROP set align_board_roll = -16 set align_board_pitch = -26 set vbat_scale = 430 set model_preview_type = 3 set applied_defaults = 5 set gps_sbas_mode = AUTO set gps_ublox_use_galileo = ON set deadband = 25 set yaw_deadband = 25 set airmode_type = THROTTLE_THRESHOLD set inav_w_z_baro_p = 0.300 set inav_w_z_gps_p = 0.500 set inav_w_z_gps_v = 0.300 set nav_use_midthr_for_althold = ON set nav_user_control_mode = CRUISE set nav_auto_speed = 350 set nav_max_auto_speed = 350 set nav_auto_climb_rate = 120 set nav_manual_speed = 350 set nav_manual_climb_rate = 120 set nav_mc_bank_angle = 15

profile

profile 1 (I only use this profile)

set mc_p_pitch = 90 set mc_i_pitch = 25 set mc_d_pitch = 100 set mc_cd_pitch = 0 set mc_p_roll = 90 set mc_d_roll = 100 set mc_cd_roll = 0 set mc_p_yaw = 100 set mc_i_yaw = 40 set mc_cd_yaw = 0 set max_angle_inclination_rll = 150 set max_angle_inclination_pit = 150 set dterm_lpf_hz = 60 set dterm_lpf_type = PT3 set nav_mc_pos_z_p = 180 set nav_mc_vel_z_p = 200 set nav_mc_vel_z_i = 60 set nav_mc_pos_xy_p = 40 set nav_mc_vel_xy_p = 30 set nav_mc_vel_xy_i = 10 set nav_mc_vel_xy_d = 80 set nav_mc_vel_xy_ff = 0 set mc_iterm_relax = RPY set d_boost_min = 0.800 set d_boost_max = 1.200 set antigravity_gain = 2.000 set antigravity_accelerator = 5.000 set smith_predictor_delay = 1.500 set thr_expo = 65 set tpa_rate = 20 set tpa_breakpoint = 1200 set rc_expo = 50 set rc_yaw_expo = 50 set roll_rate = 50 set pitch_rate = 50 set yaw_rate = 6 set manual_rc_expo = 50 set manual_rc_yaw_expo = 50

profile

profile 2

set mc_p_pitch = 44 set mc_i_pitch = 85 set mc_d_pitch = 28 set mc_i_roll = 75 set mc_d_roll = 26 set mc_p_yaw = 40 set mc_i_yaw = 80 set dterm_lpf_hz = 80 set dterm_lpf_type = PT3 set mc_iterm_relax = RPY set d_boost_min = 0.800 set d_boost_max = 1.200 set antigravity_gain = 2.000 set antigravity_accelerator = 5.000 set smith_predictor_delay = 1.500 set tpa_rate = 20 set tpa_breakpoint = 1200 set rc_expo = 75 set rc_yaw_expo = 75 set roll_rate = 70 set pitch_rate = 70 set yaw_rate = 60

profile

profile 3

set mc_p_pitch = 44 set mc_i_pitch = 85 set mc_d_pitch = 28 set mc_i_roll = 75 set mc_d_roll = 26 set mc_p_yaw = 40 set mc_i_yaw = 80 set dterm_lpf_hz = 80 set dterm_lpf_type = PT3 set mc_iterm_relax = RPY set d_boost_min = 0.800 set d_boost_max = 1.200 set antigravity_gain = 2.000 set antigravity_accelerator = 5.000 set smith_predictor_delay = 1.500 set tpa_rate = 20 set tpa_breakpoint = 1200 set rc_expo = 75 set rc_yaw_expo = 75 set roll_rate = 70 set pitch_rate = 70 set yaw_rate = 60

battery_profile

battery_profile 1

set vbat_cell_detect_voltage = 440 set throttle_idle = 10.000 set nav_mc_hover_thr = 1350

battery_profile

battery_profile 2

battery_profile

battery_profile 3

restore original profile selection

profile 1 battery_profile 1

save configuration

save

#

sensei-hacker commented 3 months ago

Current settings from the diff:

set nav_mc_pos_z_p = 180 set nav_mc_vel_z_p = 200 set nav_mc_vel_z_i = 60

Defaults:

nav_mc_pos_z_p

| Default | Min | Max | | 50 | 0 | 255 |

nav_mc_vel_z_i

I gain of velocity PID controller | Default | Min | Max | | 50 | 0 | 255 |

nav_mc_vel_z_p

| Default | Min | Max | | 100 | 0 | 255 |

nav_mc_pos_z_p appears to be currently set to 350% of the default, nav_mc_vel_z_p set to double the default. Both nearly maxed out.

You may want to test with values much closer to the defaults.

breadoven commented 3 months ago

Can you provide a log file ... would make it easier to work out what the issue is ?

Lalbabu2001 commented 3 months ago

Can you provide a log file ... would make it easier to work out what the issue is ?

Yeah sure, [Uploading blackbox_log_2024-03-21_171716.TXT…]()

Here are some images too Screenshot (262) Screenshot (261) Please review

breadoven commented 3 months ago

The log file link doesn't work, just reloads the issue page.

Lalbabu2001 commented 2 months ago

The log file link doesn't work, just reloads the issue page.

Here it is. [Uploading blackbox_log_2024-03-21_171716-1.TXT…]() https://drive.google.com/file/d/1HkbFLPIKFssm0UPSKEP5sPFZktBFC1wt/view?usp=drivesdk

breadoven commented 2 months ago

Looks like a Baro problem. In the first log you posted above, Baro altitude goes from 8.5m to 10.2m in a second even though acc[Z] remains unchanged indicating the Baro increase is false. The quad reacts by descending rather than rising as the Baro reading would indicate.

Is the Baro covered with a bit of foam to damp out prop wash affects and also is the Baro chip potentially exposed to sunlight when the quad is flying ? Direct sun on the Baro chip can cause significant fluctuations in the reading.

Lalbabu2001 commented 2 months ago

Looks like a Baro problem. In the first log you posted above, Baro altitude goes from 8.5m to 10.2m in a second even though acc[Z] remains unchanged indicating the Baro increase is false. The quad reacts by descending rather than rising as the Baro reading would indicate.

Is the Baro covered with a bit of foam to damp out prop wash affects and also is the Baro chip potentially exposed to sunlight when the quad is flying ? Direct sun on the Baro chip can cause significant fluctuations in the reading.

Here is another log with more foam over baro, the default pid setting nav_mc_pos_z_p =50 is too low and quad was loose altitude therefore I increased it to 200 . Now the quad is maintaining altitude fine, but the problem of throttle input remains same, as in new blackbox log you can see as soon I down the throttle input back to mid(1500) the quad drops some altitude. https://drive.google.com/file/d/1J-V93eOJCwLUk1sXvAC2U1Ayy097hbx3/view?usp=drivesdk Screenshot (288)

Lalbabu2001 commented 2 months ago

Looks like a Baro problem. In the first log you posted above, Baro altitude goes from 8.5m to 10.2m in a second even though acc[Z] remains unchanged indicating the Baro increase is false. The quad reacts by descending rather than rising as the Baro reading would indicate.

Is the Baro covered with a bit of foam to damp out prop wash affects and also is the Baro chip potentially exposed to sunlight when the quad is flying ? Direct sun on the Baro chip can cause significant fluctuations in the reading. Please review the recent log, I think there might be some pid or filter making this issue

Lalbabu2001 commented 2 months ago

Please review the recent log , I think there might be some pid or filter making this issue

breadoven commented 2 months ago

I think the problem here is it isn't possible to know if navPos[2] is correct or if the Baro altitude is correct in the log. navPos[2] seems to correctly follow the demanded navTgtPos[2] within a few cm's whilst Baro altitude wanders around by over a meter or more. Is the Baro altitude representative of the altitude fluctuations you're seeing, indicating navPos[2] is wrong, or the other way around ? I'm guessing the former is true.

Lalbabu2001 commented 2 months ago

So what settings to change