Version 2.2.0 and 2.2.1 POSHOLD Skyrocket

[KenImhof]

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 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]

Would be nice if you provide more details and blackbox

[KenImhof]

Blackbox log i attached.
LOG00007.zip

Link to video:
https://www.youtube.com/watch?v=uEpvAewQ7WE&feature=youtu.be

[giacomo892]

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

[KenImhof]

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]

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

[digitalentity]

Specifically, I'm interested in a value of inav_w_xyz_acc_p

[KenImhof]

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]

@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]

@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]

@KenImhof Perfect, just checking.

[digitalentity]

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

[KenImhof]

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

[digitalentity]

Awesome!