Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Version 2.2.0 and 2.2.1 POSHOLD Skyrocket #4925

Open
KenImhof opened this issue Jul 10, 2019 · 19 comments

Comments

Projects
None yet
5 participants
@KenImhof
Copy link

commented Jul 10, 2019

Current Behavior

Enabling POSHOLD while descending causes QUAD to skyrocket with motors at full throttle until NAV mode is disabled.

Steps to Reproduce

  1. Descend at a fairly fast rate
  2. Enable 3D POSHOLD mode
  3. Occurs every time and easy to reproduce

Expected behavior

QUAD should stop and stabilize at its current position.

Suggested solution(s)

Problem is caused by the new acceleration weight that was added to the navigation_pos_estimator in version 2.2.0. As a temporary solution, I disabled the accWeight calculation in the estimationPredict function in a custom build.

@issue-label-bot issue-label-bot bot added the BUG label Jul 10, 2019

@issue-label-bot

This comment has been minimized.

Copy link

commented Jul 10, 2019

Issue-Label Bot is automatically applying the label BUG to this issue, with a confidence of 0.72. Please mark this comment with 👍 or 👎 to give our bot feedback!

Links: app homepage, dashboard and code for this bot.

@giacomo892

This comment has been minimized.

Copy link
Collaborator

commented Jul 10, 2019

Would be nice if you provide more details and blackbox

@KenImhof

This comment has been minimized.

Copy link
Author

commented Jul 10, 2019

@giacomo892

This comment has been minimized.

Copy link
Collaborator

commented Jul 11, 2019

@KenImhof can you try reverting this #4448 without any other modification and test again?

@KenImhof

This comment has been minimized.

Copy link
Author

commented Jul 11, 2019

I just commented out the accWeight as listed below and my 2.2.1 quad is back-up and working without the skyrocket behavior.

static void estimationPredict(estimationContext_t * ctx)
{
const float accWeight = navGetAccelerometerWeight();

/* Prediction step: Z-axis */
if ((ctx->newFlags & EST_Z_VALID)) {
    posEstimator.est.pos.z += posEstimator.est.vel.z * ctx->dt;
    posEstimator.est.pos.z += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f; // * accWeight; KBI
    posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt; //* sq(accWeight); KBI
}

/* Prediction step: XY-axis */
if ((ctx->newFlags & EST_XY_VALID)) {
    // Predict based on known velocity
    posEstimator.est.pos.x += posEstimator.est.vel.x * ctx->dt;
    posEstimator.est.pos.y += posEstimator.est.vel.y * ctx->dt;

    // If heading is valid, accelNEU is valid as well. Account for acceleration
    if (navIsHeadingUsable() && navIsAccelerationUsable()) {
        posEstimator.est.pos.x += posEstimator.imu.accelNEU.x * sq(ctx->dt) / 2.0f; //* accWeight; KBI
        posEstimator.est.pos.y += posEstimator.imu.accelNEU.y * sq(ctx->dt) / 2.0f; //* accWeight; KBI
        posEstimator.est.vel.x += posEstimator.imu.accelNEU.x * ctx->dt; //* sq(accWeight); KBI
        posEstimator.est.vel.y += posEstimator.imu.accelNEU.y * ctx->dt; //* sq(accWeight); KBI
    }
}

}

@digitalentity

This comment has been minimized.

Copy link
Member

commented Jul 11, 2019

Can you also provide config dump of a setup that skyrockets?

@digitalentity

This comment has been minimized.

Copy link
Member

commented Jul 11, 2019

Specifically, I'm interested in a value of inav_w_xyz_acc_p

@KenImhof

This comment has been minimized.

Copy link
Author

commented Jul 11, 2019

Listed below is the diff from several days ago. Since this dump, I have changed from DSHOT to MULTISHOT and enabled AIRMODE with the same results.

diff

version

INAV/OMNIBUSF4V3 2.2.1 Jul 3 2019 / 22:13:58 (a6d8474)

GCC-8.2.1 20181213 (release) [gcc-8-branch revision 267074]

resources

mixer

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

servo mix

servo

logic

feature

feature -VBAT
feature -TX_PROF_SEL
feature -BLACKBOX
feature -AIRMODE
feature -OSD
feature GPS
feature PWM_OUTPUT_ENABLE

beeper

map

map TAER

serial

serial 0 64 115200 115200 0 115200
serial 5 2 115200 115200 0 115200

led

color

mode_color

aux

aux 0 0 1 1025 1275
aux 1 3 2 1400 1900
aux 2 9 2 1400 1900
aux 3 8 0 1875 2100
aux 4 21 3 1775 2100
aux 5 30 2 1400 1600
aux 6 31 2 1750 1950

adjrange

rxrange

temp_sensor

wp

#wp 0 invalid

osd_layout

master

set looptime = 250
set gyro_hardware_lpf = 256HZ
set acc_hardware = MPU6000
set acczero_x = 125
set acczero_y = 1
set acczero_z = 94
set accgain_x = 4062
set accgain_y = 4115
set accgain_z = 3994
set align_mag = CW270FLIP
set mag_hardware = HMC5883
set magzero_x = 75
set magzero_y = -60
set magzero_z = 28
set baro_hardware = BMP280
set pitot_hardware = NONE
set receiver_type = SERIAL
set serialrx_provider = SPEK2048
set motor_pwm_rate = 8000
set motor_pwm_protocol = DSHOT300
set failsafe_procedure = RTH
set model_preview_type = 3
set 3d_deadband_low = 1450
set 3d_deadband_high = 1550
set 3d_neutral = 1500
set nav_user_control_mode = CRUISE
set nav_manual_speed = 1500
set nav_landing_speed = 250
set nav_mc_bank_angle = 45
set nav_mc_hover_thr = 1400
set nav_fw_cruise_thr = 1000
set nav_fw_pitch2thr = 75
set nav_fw_loiter_radius = 600

profile

profile 1

set mc_p_pitch = 30
set mc_i_pitch = 25
set mc_p_roll = 30
set mc_i_roll = 25
set nav_mc_vel_xy_d = 200
set roll_rate = 60
set pitch_rate = 60
set yaw_rate = 60

battery_profile

battery_profile 1

set vbat_max_cell_voltage = 430
set vbat_warning_cell_voltage = 370

@teckel12

This comment has been minimized.

Copy link
Contributor

commented Jul 12, 2019

@KenImhof Is 1400 the correct value for this quad for hover throttle? Seems very generic. I typically calibrate this to an exact number like 1235 or whatever.

@KenImhof

This comment has been minimized.

Copy link
Author

commented Jul 12, 2019

@teckel12 this is 10-inch 3d quad with 8-inch props and the correct value is around 1650. I just did a quick non-3d setup to duplicate the problem using the standard 2.2.1 code. I just set it to 1400 to get it close for just one flight to verify the problem is with the standard build. This quad is now back to the 3d setup with a custom build with the accWeight removed and is now flying great using the base 2.2.1 code.

@teckel12

This comment has been minimized.

Copy link
Contributor

commented Jul 12, 2019

@KenImhof Perfect, just checking.

@digitalentity

This comment has been minimized.

Copy link
Member

commented Jul 12, 2019

@KenImhof can you reproduce the issue and record BB log with set debug = VIBE ?

@KenImhof

This comment has been minimized.

Copy link
Author

commented Jul 12, 2019

Sure, I will set everything back to stock values/version and should have it for you shortly.

@digitalentity

This comment has been minimized.

Copy link
Member

commented Jul 12, 2019

Awesome!

@KenImhof

This comment has been minimized.

Copy link
Author

commented Jul 12, 2019

@digitalentity attached are the log and diff files. The skyrocket occurred on the 2nd attempt in the log as it worked correct the first time. Please let me know if you would like me to try anything else and I will keep this setup available
DIFF-20190712_112507.zip
LOG00007.zip

for testing.

Thanks,

@digitalentity

This comment has been minimized.

Copy link
Member

commented Jul 12, 2019

A classic case of accelerometer resonance:
image

Accelerometer dynamic weigh safeguards significantly reduced accelerometer weight in height calculation, but not enough to entirely prevent the unexpected behavior. For your flights without the issue - you got lucky 😄

PR #4930 will give insight into accelerometer issues via OSD message.

Another action item for us - look into increasing barometer weight to allow barometer-only flight without accelerometer assistance.

@neilm06

This comment has been minimized.

Copy link

commented Jul 13, 2019

Tested out a new 12" build some days ago. Kakute F7 board with 2.2.1. Fitted a TF Mini rangefinder. Had modes set to position (i) Alt Hold + Surface. (ii) Alt Hold + Pos Hold (Surface off).
Craft took off and mode position (i) engaged at about 2m. Altitude increased to about 4m. Then engaged mode position (ii) and reduced throttle to reduce height. Craft shot upwards and tried to flip. Immediately disengaged Nav modes but craft crash landed before recovery possible. Some damage done. I put this error down to possible hardware issue until I saw this issue reported from @KenImhof. Have repaired craft without the TFmini installed and set single Nav mode of Pos Hold.

Can I be confident please that 2.2.1 on Kakute F7 is unrelated to the problem and all Nav modes will work correctly?

@digitalentity

This comment has been minimized.

Copy link
Member

commented Jul 13, 2019

@neilm06 we can't debug the issue without blackbox log. Flipping in the air (aka "roll of death") is generally a hardware issue, but it's impossible to tell without blackbox. Poshold, althold and surface mode are verified to be working in 2.2.1

@neilm06

This comment has been minimized.

Copy link

commented Jul 13, 2019

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.