How to turn off the cyclic blade pitch and collective pitch when the helicopter is on the ground

Yes, our situation is quite extreme. The vibration of a real drone is very high, which also bothers us. We want to make progress on this, but it is too difficult at the moment. Thank you for your patient reply. I am very happy to get your attention and look forward to it.

Update

//set servo positions when !interlock and armed  
if(!_flags.interlock && _flags.armed){
     _servo_out[CH_1] = ((float)(_collective_min - 1500))/500.0f;
     _servo_out[CH_2] = ((float)(_collective_min - 1500))/500.0f;
     _servo_out[CH_3] = ((float)(_collective_min - 1500))/500.0f;
     _servo_out[CH_4] = ((float)(_collective2_min - 1500))/500.0f;
     _servo_out[CH_5] = ((float)(_collective2_min - 1500))/500.0f;
     _servo_out[CH_6] = ((float)(_collective2_min - 1500))/500.0f;
}

@Flying2015 You DO NOT want to go about it like this. You are messing with the lower level parts of the code. I think I understand the issue as to why it is happening. Just need to get Leonard onboard and come up with a suitable solution. This won’t be an overnight fix but we will get to it.

Okay, I’m in a hurry. Thank you for your advice and attention. Your work is significant, and we are working together to make this automatic control system better.

I can show you what the issue is. You can help fix it the right way

That’s great. I’d appreciate your advice.

I submitted a PR for this here. I will have @Leonardthall look at it. This is my thought as the best way to fix this issue.

Wow, great, I’ll visit and learn it right away. I’m very grateful and touched that you stayed up late for this. Thanks againg.

Ok, so I have been able to look at this collective output problem. This is in two parts.

The first part is that your high noise levels mean your measured acceleration significantly exceeds your maximum acceleration limit so when the controller initialises the acceleration it does not initialise it to your measured acceleration (±3 g or 30m/s/s) but instead it is limited to 2.5m/s/s. Therefore the following loop sees a large acceleration error and this results in a significant collective command just due to the P term in the acceleration PID loop. This part is not a bug.

The second part is what Bill noticed. The I term relaxed by taking the previous output and reducing it. However this assumes that the previous output had no output due to the P term and is therefore constant apart from the slowly reducing I term. This is a bug in that we should not assume this output has no P term and instead it should be based on the previous I term.

I have a fix for the I term problem here with a cleaned up handling of the reset_I_smoothly using a new relax_integrator function. Here:

Now this does not fix your problem as it is fundamentally caused by stupidly large vibrations getting to the position controller. These changes will help reduce your problem a little but the real fix is to:

  1. Reduce the vibrations getting to your autopilot
  2. Reduce the magnitude of the vibrations getting to the controller

I can’t help you with 1 but I can help you with 2. Your current setting of the acceleration filter is very high:
INS_ACCEL_FILTER 20 Hz

It looks like your acceleration noise is approximately 5 Hz. You could reduce your INS_ACCEL_FILTER setting to 0.5 to 1 Hz to greatly reduce this noise at the controller level. However we need to ensure that the lower controller response does not make Alt_Hold unstable. If so we may need to reduce the PSC Z gains.

I am honored to receive your reply. I will study your suggestions carefully. Yes, I agree with you and Bill. The vibration of our helicopter is really high, especially when the ground resonates. This may have something to do with the fact that it is powered by an internal combustion engine. We’re trying to figure out how to optimize.

Thank you very much for your advice and attention. Your suggestion on filter frequency is very meaningful, I will try it out as soon as possible. After Bill put forward the suggestion about PSC_ACCZ_FLTE, I have also reduced the INS_ACCEL_FILTER to 10Hz. I will continue to reduce the frequency of INS_ACCEL_FILTER according to your suggestion. If the filtering frequency is too low, will there be a delay?

Best Wishes. Happy New Year!

The lower the filter setting the lower the delay but also the lower the noise that will reach the controller. As long as your noise magnitude is less than 2.5m/s/s then your controller will initialise correctly.

If it is too low you will see the aircraft oscillate up and down. To fix this you will need to reduce the PSC_ACCEL_Z parameters.

So make sure you are confident taking over in stabilize mode.

Do you mean PSC_ACCEL_Z parameters by PSC_ACCZ_P? Or all the parameters that depend on Z?

Besides,also do you think we need to lower the INS_GYRO__FILTER?

Yes, those. In particular the P and I term.

Thank you for your prompt reply,I Got it. Also do you think we need to lower the INS_GYRO__FILTER?

That is a separate tuning issue so I would not touch that.

I got it. Thanks for your time.

@bnsgeyer Hello, Bill. Copter-4.1.4 has been released. Are the amendments (Position controller init fix to avoid twitch on vehicles with high vibrations) referred to in it specific to the problem we are discussing here?

I copied the ReleaseNotes.

Copter-4.1.4 has been released as the official/stable version for multicopters and helicopters.

The changes vs 4.1.3 are in the ReleaseNotes and also copied below

  1. Benewake CAN Lidar support
  2. CAN GPS default lag dropped to 0.1 seconds (was 0.22 seconds)
  3. Bug fixes
    a) Compass custom orientation is never overwritten during calibration
    b) EKF logging gaps fixed (some messages were occasionally being skipped)
    c) Guided mode cornering improvements
    d) IMU logging fix for IREG message (records IMU register changes)
    e) LOITER_TO_ALT mission command’s climb rate fixed (could climb or descend too quickly)
    f) Position controller init fix to avoid twitch on vehicles with high vibrations
    g) Position controller init fix to better handle high speed entry to flight mode
    h) Position controller prioritises reducing cross track error
    i) Position controller relax fix
    j) SBUS RC frame gap increased to better handle some new receivers
    k) SERVOx_FUNCTION protection to avoid memory overwrite if set too high
    l) SD card init triggering watchdog fixed
    m) Spline path max lateral acceleration reduced (vehicle stays on path better but may be slower)
    n) Takeoff bug fix if taking off below home or frame specified as MSL

Yes this version fixes the issue you were having with the vertical axis with high vibrations.

1 Like

Nice!Thank you very much.

@bnsgeyer @Leonardthall I had a test yesterday and the problems mentioned on this topic have been solved. Thank you very much.

1 Like