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

Discussion - Acro flight performance enhancements #218

Closed theArchLadder closed 8 years ago

theArchLadder commented 8 years ago

I just tried betaflight, wow! Very impressive, so locked in compared to fp-pid. It felt like i became a twice as good pilot!

Could we get this performance in inav? Why having to compromise between nav and acro performance! :stuck_out_tongue_winking_eye: The default looptime on betaflight is 500 and even i could notice a difference when switching to 1000 like i have on inav. 500 made it feel a little bit more sharp. Would it be possible to get inav to run at 500 on F3 targets?

But the most important difference is that the attitude was so locked in! Perhaps i'm a terrible tuner but no matter what i do i can't get the attitude to be locked in on fp-pid, and to high I-gains starts to ruin the performance... Is there to much anti-windup or what is going on here? Attitude on betaflight was very locked in even on 1000 looptime. Fp-pid changes the attitude all the time without me telling it to, that makes acro flying very hard.

As usual i'm more than happy to make my custom builds and flight test them, just give me some ideas on what to try! What else could we borrow from betaflight to improve acro flight?

DzikuVx commented 8 years ago

I'm trying to find an information how much delay BiQuad filter would induce for 8kHz mode. I've found delay information for MPU6050

martinbudden commented 8 years ago

@digitalentity , I think we are thinking along similar lines here, perhaps with some differences in the implementation details.

But my work on this is firmly on hold a the moment - need to get the F4 port and the parameter groups done first...

digitalentity commented 8 years ago

@martinbudden I'm putting all further improvements on hold until F4 and Param Groups are implemented, to prevent conflicts.

DzikuVx commented 8 years ago

@martinbudden @digitalentity @theArchLadder after replacing faulty motor I did some experiments with gyro_soft_lpf_hz on my big machine: 12" props, 600mm, 1.8kg. And results are well, interesting.

Looks like the same principle applies to small and big machines after all. Last entry from blackbox log shows what is happening at gyro_soft_lpf_hz=80 and gyro_lpf=188HZ, gyro_sync=ON and gyro_sync_denom=2. OK, gyros and motors are quite jumpy and far from clean, but quad flies very well. Is responsive, holds attitude, descends with only small wobble.

What is even stranger, current cutoff is above motor noise at hover (around 75Hz). Looks like, gyro noise has less effect that signal delay. With gyro_lpf=188HZ and gyro_soft_lpf_hz=50 or even gyro_soft_lpf_hz=45 gyro traces looked better, but _P_ gains had to be much much higher (around x1.5 higher) to archive similar effects. But then quad was very machine-like.

Probably huge inertia of 12" props acts as super efficient LPF filter that breaks feedback loop PID->motor->gyro->PID

P = 85
I = 16
D=33

My next conclusion is that high I gain is needed only on acrobatic machines. On big machines, where stability is more important than flips or roll (who does flips on 2kg UAV?) low I gain gives much better results.

Also, in such non-acribatic machines D term has very little impact on performance. D term trace (not counting noise) is almost all the time flat.

bb_krogulec_lpf_wysoko.TXT

DaleKramer commented 8 years ago

This is a great discussion! I am about to start trying to tune my 6 kg sort of hexacopter http://www.vlazair.com/projectStatus.html (only 1.3/1 thrust to weight), any suggestions on where I should start with all those parameters you are playing with?

DzikuVx commented 8 years ago

@DaleKramer I would say start with defaults. I see you have some fancy design over there, so nobody knows how it's gonna behave. At least in vertical flight. About transition to horizontal... hmm... I really do not think INAV is prepared to do smth like this...

DaleKramer commented 8 years ago

Yep, but someone has to try ;). I am just thinking 80 degree angle mode will let me transition to horizontal flight and a small aerodynamic zoom to high alpha and then start up the electric motors will work for transition back to hover. I will use idle up to start/stop electric motors and FC.

digitalentity commented 8 years ago

@DaleKramer wow, that's a fancy design! You are very much welcome to bring VTOL capabilities to INAV :smiley: At the moment INAV doesn't have the capability to transition from multirotor mixer to airplane mixer as they use different servo mapping, but that's something we should account for. VTOLs are great!

DaleKramer commented 8 years ago

I am operating my aircraft servos in parallel with FC commands, using TREA123 stick mode so hovering uses same logic (frame of reference) as 3D hover in aircraft. FC will be off using idle up OFF in horizontal flight.

digitalentity commented 8 years ago

Hm... using simple airplane mixer might work. Make servos and motors work together in vertical flight using some smart mixing. Interesting.

DaleKramer commented 8 years ago

Hoping to start tuning pitch axis on a test stand very soon, time will tell.

gurkenfolie commented 8 years ago

i checked my bb log, D goes crazy with gyro_lpf = 256HZ, when set to the default value everything is fine but P has to be lowered a lot. with gyro_lpf = 256HZ the quad flies better but the motors are somewhat nervous.

theArchLadder commented 8 years ago

@gurkenfolie You can lower these: gyro_soft_lpf_hz = 75 dterm_lpf_hz = 50 But it sounds like you have to much vibrations, fixing that would be even better.

gurkenfolie commented 8 years ago

gyro_soft_lpf set to 60 helped. i will give it a new try with new balanced propellers and one motor has to be changed, maybe this causes the vibrations.

skaman82 commented 8 years ago

Just testet the latest master with my 250 quad with 6" props. Angle, Horizon and GPS modes a working really good but it is practically unflyable in acro. Tried to change some settings to

tried my 250 with this settings gyro_lpf = 256HZ gyro_sync_denom=12 gyro_soft_lpf_hz = 75 dterm_lpf_hz = 50 F3, looptime 2000

but it made things much worse.

theArchLadder commented 8 years ago

@skaman82 "worse" doesn't tell us much, in what way was it worse? Did it not feel locked in? Did you have oscillations? Was the stick to fast/slow?

skaman82 commented 8 years ago

Well no oscillations but it flies like a drunken sailor. Hard to describe The quad completely breaks out in fast maneuvers and yaw turns. It is very opposite of locked in.

skaman82 commented 8 years ago

With changes i posted above it was absolutley not flyeble at all. Even in stabilizised modes. I barely could control it

theArchLadder commented 8 years ago

@skaman82 Sounds very strange, can you post a full dump please?

Ralf-W commented 8 years ago

Please post your complete CLI dump. Then we can see if all parameters fit. E.g. did you "set gyro_sync = ON"? On my Naze32 "gyro_sync_denom=12" is resulting in a looptime of 1500 but you post looptime 2000.

skaman82 commented 8 years ago

Here is my dump and my blackbox log - but I'm sure it run out of flash space BEFORE I tested Acro and the custom settings I mentioned above:

version

INAV/SPRACINGF3 1.2.0 Jul 16 2016 / 21:18:21 ()

pflags

Persistent config flags: 0x00000001

dump master

mixer

mixer QUADX mmix reset smix reset

feature

feature -RX_PPM feature -VBAT feature -INFLIGHT_ACC_CAL feature -RX_SERIAL feature -MOTOR_STOP feature -SERVO_TILT feature -SOFTSERIAL feature -GPS feature -FAILSAFE feature -SONAR feature -TELEMETRY feature -CURRENT_METER feature -3D feature -RX_PARALLEL_PWM feature -RX_MSP feature -RSSI_ADC feature -LED_STRIP feature -DISPLAY feature -ONESHOT125 feature -BLACKBOX feature -CHANNEL_FORWARDING feature -RX_NRF24 feature -SOFTSPI feature RX_PPM feature VBAT feature MOTOR_STOP feature GPS feature FAILSAFE feature LED_STRIP feature BLACKBOX

beeper

beeper GYRO_CALIBRATED beeper RX_LOST beeper RX_LOST_LANDING beeper DISARMING beeper ARMING beeper ARMING_GPS_FIX beeper BAT_CRIT_LOW beeper BAT_LOW beeper GPS_STATUS beeper RX_SET beeper ACC_CALIBRATION beeper ACC_CALIBRATION_FAIL beeper READY_BEEP beeper MULTI_BEEPS beeper DISARM_REPEAT beeper ARMED beeper SYSTEM_INIT beeper ON_USB

map

map AETR1234

serial

serial 0 1 115200 57600 0 115200 serial 1 1 115200 57600 0 115200 serial 2 2 115200 115200 0 115200

led

led 0 6,1::WC:6 led 1 6,0::C:6 led 2 7,0::C:6 led 3 8,0::C:6 led 4 9,0::C:6 led 5 10,0::C:6 led 6 10,14::WC:2 led 7 10,15::AC:2 led 8 9,15::C:2 led 9 8,15::C:2 led 10 7,15::C:2 led 11 6,15::AC:2 led 12 0,0:::0 led 13 0,0:::0 led 14 0,0:::0 led 15 0,0:::0 led 16 0,0:::0 led 17 0,0:::0 led 18 0,0:::0 led 19 0,0:::0 led 20 0,0:::0 led 21 0,0:::0 led 22 0,0:::0 led 23 0,0:::0 led 24 0,0:::0 led 25 0,0:::0 led 26 0,0:::0 led 27 0,0:::0 led 28 0,0:::0 led 29 0,0:::0 led 30 0,0:::0 led 31 0,0:::0

color

color 0 0,0,0 color 1 0,255,255 color 2 0,0,255 color 3 30,0,255 color 4 60,0,255 color 5 90,0,255 color 6 120,0,255 color 7 150,0,255 color 8 180,0,255 color 9 210,0,255 color 10 240,0,255 color 11 270,0,255 color 12 300,0,255 color 13 330,0,255 color 14 0,0,0 color 15 0,0,0

set looptime = 2000 set emf_avoidance = OFF set i2c_overclock = OFF set gyro_sync = OFF set gyro_sync_denom = 2 set mid_rc = 1500 set min_check = 1100 set max_check = 1900 set rssi_channel = 0 set rssi_scale = 30 set rssi_ppm_invert = OFF set rc_smoothing = ON set input_filtering_mode = OFF set min_throttle = 1150 set max_throttle = 1850 set min_command = 1000 set servo_center_pulse = 1500 set 3d_deadband_low = 1406 set 3d_deadband_high = 1514 set 3d_neutral = 1460 set 3d_deadband_throttle = 1000 set motor_pwm_rate = 400 set servo_pwm_rate = 50 set disarm_kill_switch = ON set auto_disarm_delay = 5 set small_angle = 35 set reboot_character = 82 set gps_provider = UBLOX set gps_sbas_mode = NONE set gps_dyn_model = AIR_1G set gps_auto_config = ON set gps_auto_baud = ON set inav_auto_mag_decl = ON set inav_accz_unarmedcal = ON set inav_use_gps_velned = ON set inav_gps_delay = 200 set inav_gps_min_sats = 6 set inav_w_z_baro_p = 0.350 set inav_w_z_gps_p = 0.200 set inav_w_z_gps_v = 0.200 set inav_w_xy_gps_p = 1.000 set inav_w_xy_gps_v = 2.000 set inav_w_z_res_v = 0.500 set inav_w_xy_res_v = 0.500 set inav_w_acc_bias = 0.010 set inav_max_eph_epv = 1000.000 set inav_baro_epv = 100.000 set nav_disarm_on_landing = ON set nav_use_midthr_for_althold = OFF set nav_extra_arming_safety = OFF set nav_user_control_mode = ATTI set nav_position_timeout = 5 set nav_wp_radius = 100 set nav_max_speed = 300 set nav_manual_speed = 500 set nav_manual_climb_rate = 200 set nav_landing_speed = 200 set nav_land_slowdown_minalt = 500 set nav_land_slowdown_maxalt = 2000 set nav_emerg_landing_speed = 500 set nav_min_rth_distance = 500 set nav_rth_tail_first = OFF set nav_rth_alt_mode = AT_LEAST set nav_rth_altitude = 1000 set nav_mc_bank_angle = 30 set nav_mc_hover_thr = 1500 set nav_mc_auto_disarm_delay = 2000 set nav_fw_cruise_thr = 1400 set nav_fw_min_thr = 1200 set nav_fw_max_thr = 1700 set nav_fw_bank_angle = 20 set nav_fw_climb_angle = 20 set nav_fw_dive_angle = 15 set nav_fw_pitch2thr = 10 set nav_fw_roll2pitch = 75 set nav_fw_loiter_radius = 5000 set serialrx_provider = SPEK1024 set spektrum_sat_bind = 0 set telemetry_switch = OFF set telemetry_inversion = ON set frsky_default_lattitude = 0.000 set frsky_default_longitude = 0.000 set frsky_coordinates_format = 0 set frsky_unit = IMPERIAL set frsky_vfas_precision = 0 set hott_alarm_sound_interval = 5 set battery_capacity = 0 set vbat_scale = 110 set vbat_max_cell_voltage = 43 set vbat_min_cell_voltage = 33 set vbat_warning_cell_voltage = 35 set current_meter_scale = 400 set current_meter_offset = 0 set multiwii_current_meter_output = OFF set current_meter_type = ADC set align_gyro = DEFAULT set align_acc = DEFAULT set align_mag = DEFAULT set align_board_roll = -10 set align_board_pitch = 30 set align_board_yaw = 0 set gyro_lpf = 42HZ set moron_threshold = 32 set imu_dcm_kp = 2500 set imu_dcm_ki = 50 set imu_dcm_kp_mag = 10000 set imu_dcm_ki_mag = 0 set pos_hold_deadband = 20 set alt_hold_deadband = 50 set yaw_motor_direction = 1 set yaw_jump_prevention_limit = 200 set tri_unarmed_servo = ON set servo_lowpass_freq = 400 set servo_lowpass_enable = OFF set failsafe_delay = 10 set failsafe_off_delay = 200 set failsafe_throttle = 1000 set failsafe_kill_switch = OFF set failsafe_throttle_low_delay = 100 set failsafe_procedure = SET-THR set rx_min_usec = 885 set rx_max_usec = 2115 set acc_hardware = 0 set baro_use_median_filter = ON set baro_hardware = 0 set mag_hardware = 0 set blackbox_rate_num = 1 set blackbox_rate_denom = 1 set blackbox_device = SPIFLASH set magzero_x = 327 set magzero_y = -443 set magzero_z = 207 set acczero_x = 62 set acczero_y = -14 set acczero_z = 68 set accgain_x = 4104 set accgain_y = 4083 set accgain_z = 4031

rxfail

rxfail 0 a rxfail 1 a rxfail 2 a rxfail 3 a rxfail 4 h rxfail 5 h rxfail 6 h rxfail 7 h rxfail 8 h rxfail 9 h rxfail 10 h rxfail 11 h rxfail 12 h rxfail 13 h rxfail 14 h rxfail 15 h rxfail 16 h rxfail 17 h

dump profile

profile

profile 0

aux

aux 0 1 0 900 1300 aux 1 2 0 1300 1700 aux 2 3 1 1300 1700 aux 3 10 2 1300 1700 aux 4 9 2 1700 2100 aux 5 12 2 1700 2100 aux 6 14 1 1700 2100 aux 7 0 0 900 900 aux 8 0 0 900 900 aux 9 0 0 900 900 aux 10 0 0 900 900 aux 11 0 0 900 900 aux 12 0 0 900 900 aux 13 0 0 900 900 aux 14 0 0 900 900 aux 15 0 0 900 900 aux 16 0 0 900 900 aux 17 0 0 900 900 aux 18 0 0 900 900 aux 19 0 0 900 900

adjrange

adjrange 0 0 0 900 900 0 0 adjrange 1 0 0 900 900 0 0 adjrange 2 0 0 900 900 0 0 adjrange 3 0 0 900 900 0 0 adjrange 4 0 0 900 900 0 0 adjrange 5 0 0 900 900 0 0 adjrange 6 0 0 900 900 0 0 adjrange 7 0 0 900 900 0 0 adjrange 8 0 0 900 900 0 0 adjrange 9 0 0 900 900 0 0 adjrange 10 0 0 900 900 0 0 adjrange 11 0 0 900 900 0 0

rxrange

rxrange 0 1000 2000 rxrange 1 1000 2000 rxrange 2 1000 2000 rxrange 3 1000 2000

servo

servo 0 1000 2000 1500 90 90 100 -1 servo 1 1000 2000 1500 90 90 100 -1 servo 2 1000 2000 1500 90 90 100 -1 servo 3 1000 2000 1500 90 90 100 -1 servo 4 1000 2000 1500 90 90 100 -1 servo 5 1000 2000 1500 90 90 100 -1 servo 6 1000 2000 1500 90 90 100 -1 servo 7 1000 2000 1500 90 90 100 -1

set nav_alt_p = 50 set nav_alt_i = 0 set nav_alt_d = 0 set nav_vel_p = 100 set nav_vel_i = 50 set nav_vel_d = 10 set nav_pos_p = 65 set nav_pos_i = 120 set nav_pos_d = 0 set nav_posr_p = 180 set nav_posr_i = 15 set nav_posr_d = 100 set nav_navr_p = 10 set nav_navr_i = 5 set nav_navr_d = 8 set deadband = 5 set yaw_deadband = 5 set throttle_tilt_comp_str = 0 set default_rate_profile = 0 set gimbal_mode = NORMAL set mag_declination = 240 set mag_hold_rate_limit = 90 set p_pitch = 40 set i_pitch = 30 set d_pitch = 30 set p_roll = 35 set i_roll = 30 set d_roll = 30 set p_yaw = 70 set i_yaw = 45 set d_yaw = 0 set p_level = 20 set i_level = 15 set d_level = 75 set max_angle_inclination_rll = 300 set max_angle_inclination_pit = 300 set gyro_soft_lpf_hz = 60 set acc_soft_lpf_hz = 15 set dterm_lpf_hz = 40 set yaw_lpf_hz = 30 set yaw_p_limit = 300

dump rates

rateprofile

rateprofile 0

set rc_expo = 70 set rc_yaw_expo = 20 set thr_mid = 50 set thr_expo = 0 set roll_rate = 50 set pitch_rate = 50 set yaw_rate = 40 set tpa_rate = 0 set tpa_breakpoint = 1500

blackbox_log_2016-07-17_205405.TXT

skaman82 commented 8 years ago

@Ralf-W setting gyro_sync_denom=12 does'n changed my looptime at all. It was still 2000. I didn't touched gyro_sync. It is OFF on default

theArchLadder commented 8 years ago

@skaman82 gyro_sync should be ON if you want to use gyro_sync_denom. Not sure why ON is not the default, it seems to be the better option...?

Ralf-W commented 8 years ago

If I remember correctly, for INAV looptime was set to a fixed value of 2000 for F1 processors to obtain a predictable behaviour even if users activate a lot of features in parallel (like LED, Display, etc.). At that time, gyro_sync was set to OFF as default. However, with this issue here, discussion and experiments started to disable the hardware lpf and use software filtering to a higher extend. Doing this, it is a must to use gyro_sync = ON with a gyro_sync_denom that the FC can handle. For my Naze32 I can use gyro_sync_denom = 12 resulting in a looptime of 1500. It was tested that the minimum looptime for the F1 processor is about 1400. Boards with F2 (or F4) processors can handle much shorter looptimes and thus smaller gyro_sync_denom.

Edit: @skaman82 In your dump I find "set gyro_lpf = 42HZ" instead of "set gyro_lpf = 256HZ". With hardware filter set to 42HZ plus the softfilters discussed here, your copter will indeed become difficult to handle.

WaspFPV commented 8 years ago

How about changing defaults to: set gyro_lpf = 188hz set gyro_soft_lpf = 60 set gyro_sync = ON set gyro_sync_denom = 2 (F1) or 1 (F3)

That should fly pretty much like cleanflight out of the box. The default of 42hz has too much latency imo. mpu_latency (thanks to @borisbstyle for the image)

theArchLadder commented 8 years ago

@andre92 I did notice a big difference when going from 188hz to 256hz (i think 256hz actually just disables the hardware filter).

WaspFPV commented 8 years ago

@theArchLadder 256hz in Inav is the same setting as OFF in CF/BF and it still filters at 256hz. The only setting that completely disables the filter is "Experimental" in betaflight but Boris doesn't recommend it, and BF uses 256hz by default. I think it would be too radical for a default on Inav, especially since current default is 42hz. Also, Inav doesn't run below 1khz, although it is nice to run 1500 looptime with 256hz on an F1 if you use ppm receiver and gps on uart2. (that is pretty much the limit of an F1, I had to enable emf_avoidance to get it to fly)

theArchLadder commented 8 years ago

@andre92 Using 256hz and relying on the software filters instead seems to give better performance. Perhaps it has something to do with the gyro running at 8khz...

In my testing with switching from 188hz to 256hz i could increase P-gain from 6.6 to 8.4 before constant oscillations, that's a huge difference, and the software filters still does a good job of filtering the noise.

Would be interesting to hear from more users trying 256hz and tuning the software filters on bigger copters... Anyone willing to try? @stronnag perhaps? @DzikuVx did you only test with 188hz?

DzikuVx commented 8 years ago

@theArchLadder @andre92 I'm flying

gyro_lpf = 256HZ
gyro_sync=ON
gyro_sync_denom=12
gyro_soft_lpf_hz = 75
dterm_lpf_hz = 50

on SPRacingF3. Super nice experience

theArchLadder commented 8 years ago

@DzikuVx Is that on your 250? Have you tried it on anything bigger?

stronnag commented 8 years ago

@DzikuVx

gyro_sync_denom=12

Isn't that loop time = 12000 ?

WaspFPV commented 8 years ago

@stronnag with gyro_lpf at 256hz, gyro runs at 8khz, with 125us interval instead of 1ms, so 12*125 will be 1500 @DzikuVx can an SPR F3 run Inav at 1khz? just curious. I make 1500 looptime on a Naze32 (F1). What is your cpu load with that looptime?

stronnag commented 8 years ago

gotcha. How confusing.

theArchLadder commented 8 years ago

@andre92 I run my F3 at 1khz. I think the minimum looptime is something like 600-700us

DzikuVx commented 8 years ago

@andre92 I was unable to use 1kHz on F3 due to some ESC glitch on OneShot125 (Emax ESC). Motors were spinning on boot, so back 1.5kHz.

@theArchLadder on 600mm quad I use 188HZ and looptime=2000

theArchLadder commented 8 years ago

@DzikuVx was 256hz bad on your 600mm or have you just not tried it yet?

DzikuVx commented 8 years ago

Did not tried yet. And probably will not try soon since my FC got burned there.

Ralf-W commented 8 years ago

I am experimenting with my "large" tricopter (Distance front motors to tail motor: 93 cm; 766 gr) and “set gyro_lpf = 256HZ” with a Naze32 (F1). Looptime is 1500. At the moment I have set the soft_lpf_hz to very low values. But for me, it performs excellently in smooth flight and in smooth acrobatics.

Here are two Flips. First graph is gyro(roll) and rc_command(roll). Second graph is pid_sum(roll) plus P,I,D (roll). flip

INAV/NAZE 1.1.0 May 11 2016 / 02:37:17 (1f49891) Persistent config flags: 0x00000001 mixer TRI set looptime = 2000 set emf_avoidance = OFF set i2c_overclock = OFF set gyro_sync = ON set gyro_sync_denom = 12 set rc_smoothing = ON set input_filtering_mode = OFF set small_angle = 25 set gyro_lpf = 256HZ set deadband = 5 set yaw_deadband = 5 set p_pitch = 50 set i_pitch = 20 set d_pitch = 180 set p_roll = 50 set i_roll = 30 set d_roll = 180 set p_yaw = 120 set i_yaw = 90 set d_yaw = 1 set p_level = 120 set i_level = 15 set d_level = 75 set max_angle_inclination_rll = 300 set max_angle_inclination_pit = 300 set gyro_soft_lpf_hz = 30 set acc_soft_lpf_hz = 15 set dterm_lpf_hz = 15 set yaw_lpf_hz = 30 set yaw_p_limit = 300

DzikuVx commented 8 years ago
  1. I would say you have too much gyro noise, even with low lpf PID output looks very noisy. Balance props
  2. 30Hz is very low.
  3. Remember about inertia. motors far from COG have huge inertia when they start to rotate and it's hard to stop them. Delay between command and execution (roll stop) is normal
WaspFPV commented 8 years ago

I suspect the overshoot is a result of overfiltering, as @DzikuVx points out. I have the same issues with my big noisy quadcopter. I wouldn't set gyro_soft_lpf_hz below 50 even with gyro_lpf = 256hz and a naze rev6, instead I'd try soft mounting the flight controller or troubleshooting the vibrations.

stronnag commented 8 years ago

Just for fun I tried a couple of things on my 420mm quad and 600mm tri (both previously on defaults of gyro_lpf 98Hz, gyro_sync=2)

On the quad, first used @DzikuVx's 256HZ / sync=12 et al settings. Flew manual, PH, RTH, WPs. Didn't really feel much improvement, and the downside was that landing detection was no longer instant (like taking 3-5 seconds on 500ms landing shutdown delay).

I then when to 188HZ / sync=1 (1000 loop). Again manual, PH, RTH, WPs. To my somewhat tame flying style, I liked it. And landing detection is good again. This is now my personal default on F3. Worked well on the 600mm tri as well, again in all flight modes.

theArchLadder commented 8 years ago

@stronnag That landing detection behavior sounds strange, could you share a bblog on that?

stronnag commented 8 years ago

Unfortunately not, the 256HZ log was largely corrupt. Very annoying. However it was consistent over two attempts.

WaspFPV commented 8 years ago

@stronnag, if you can use sync_denom = 1 with 188hz on a F3, you should use gyro_sync_denom = 8 with gyro_lpf = 256hz you should be able to keep 1000 looptime on F3. gyro_sync_denom = 12 is for F1 controllers. (or =2 when using 188hz or lower)

theArchLadder commented 8 years ago

@stronnag What values did you use for gyro_soft_lpf_hz and dterm_lpf_hz? I guess it makes sense to use lower values on a bigger copter (slower spinning props).

DzikuVx commented 8 years ago

@theArchLadder I also thought so, but well, I was wrong. As long as gyro noise is not making terrible things, it's worth to keep bot as high as possible. Just to find a sweet spot between delay and noise

DzikuVx commented 8 years ago

Everyone interested in this issue might want to try #369

WaspFPV commented 8 years ago

369 looks good, but the motor output is still 666hz but with 2khz gyro sampling? I noticed in Betaflight the attitude estimation has been seperated from the pid loop (only 100hz and it still works), so task_pid is really only the acro controller. Is it possible to split the Inav pid task so attitude and position estimation are seperated and only the fp-pid function is treated as the "real" pid loop? That way maybe the gyro/pid and motor output can run 1khz with attitude estimation and position estimation every other loop.

Something like this: (should work even on F1 targets) 1khz:

100-500hz:

Since fp-pid has a smoothing filter on angleRate, I don't think it will fly jerky at lower angle updates.

DzikuVx commented 8 years ago

@andre92 OK, this deserves few longer sentences.

  1. I'm working in a separate branch on something that I all "Race" feature. When enabled it will sacrifice as much as possible to give as much processing power for gyro/pid loop. One of the things will be acc running at much lower frequency (120Hz) but also with faster (less precise) ACC filtering. And maybe I will find some other things that can be disabled but will give some extra performance to gyros
  2. Betaflight does ACC sampling at 100Hz
  3. We can not do ACC at 100Hz in case of GPS assisted modes. Not enough resolution. That's why "Race" mode will have so much acc support as required to do emergency hover in Attitude or show artificial horizon on OSD
  4. This might sound as heresy, but I do not belive updating motors above 500Hz makes much difference. OK, maybe 1kHz. Why? Because I did some math. Imagine a 5" prop running at 12000RPM and 2kHz motor update. And now, guess what is the distance prop tip travels in 500us (2kHz). It is only 40mm. more less 10% of a full rotation. How much power would be required to considerably change speed and thrust on a distance of 36 degrees? Even with Active Breaking. Motors have limited power. Even ESC shows some problems here. By default most of them runs motor on 8kHz. That really does not give much room to change power delivered to motors in a smooth way.
  5. But, BF running main pid loop very fast, at 2kHz or even 8kHz have some different advantages besides motor:
    1. faster gyro update what we are doing now in #369
    2. Less delay
    3. better filtering
  6. Running 8kHz gyro on I2C connected gyros is rather no-no. Bus not fast enough without DMA. And we do not have DMA ATM. In case of SPI it is better
  7. I will try to get more, but gyro at 1kHz for F1 and 2.6kHz on I2C F3 is all we can do.
WaspFPV commented 8 years ago

I just checked acc sampling in Betaflight, it still runs faster than attitude loop, at 333hz on my Naze. My bad, I thought it was 1khz, as it should be. I think in Inav it should be at least 500hz as it is now.

Personally I don't need 2khz or more, but 1khz feels just a little more locked in than 500hz, especially in descents. For smaller copters this may be more noticable because of higher Nyquist frequencies.

I will do some testing and see what is possible on a Naze. I was able to achieve 1500us looptime, so I think there is room for one extra gyro + pid update in 500us.

I think I'm going to break up the main pid loop into 3 tasks: gyro+pid, acc+attitude and positioning+navigation, and see how that works.