Inav: RTH procedure got stuck at initial climb phase

[krzysztofmatula]

I engaged RTH at altitude ~7m. The quad entered pos-hold, made a yaw turn towards home and... got stuck in such state. I was waiting ~40s and nothing - just hanging in pos and alt hold, not climbing to 10m and not moving towards home.

See the log 3/3 in the attached file, from 7:05 to 7:45:
blackbox_log_2017-04-22_174906.TXT

# diff

# version
# INAV/SPRACINGF3 1.6.1 Mar 28 2017 / 18:51:24 (a3c93f9)

# resources

# mixer

# servo

# servo mix

# feature
feature SOFTSERIAL
feature GPS
feature TELEMETRY
feature RSSI_ADC
feature LED_STRIP
feature PWM_OUTPUT_ENABLE

# beeper
beeper -ON_USB

# map

# serial
serial 0 1 115200 57600 0 115200
serial 1 2 115200 57600 0 115200
serial 2 16 57600 57600 57600 115200
serial 31 1 9600 38400 0 115200

# led
led 0 15,15:S:C:2
led 1 14,15:S:CW:2
led 2 9,15:S:CW:2
led 3 8,15:S:CW:2
led 4 7,15:S:CW:2
led 5 6,15:S:CW:2
led 6 1,15:S:CW:2
led 7 0,15:S:C:2
led 8 11,0:N:C:1
led 9 10,0:N:CW:1
led 10 9,0:N:CW:1
led 11 8,0:N:CW:1
led 12 7,0:N:CW:1
led 13 6,0:N:CW:1
led 14 5,0:N:CW:1
led 15 4,0:N:C:1

# color

# mode_color

# aux
aux 0 2 1 900 1200
aux 1 4 1 900 1075
aux 2 4 1 1250 1350
aux 3 3 0 1100 1250
aux 4 3 0 1350 1475
aux 5 3 0 1600 1750
aux 6 3 0 1900 2100
aux 7 9 0 1250 1500
aux 8 8 0 1500 2100
aux 9 11 1 1500 2100
aux 10 12 2 1700 2100
aux 11 18 1 1500 2100

# adjrange

# rxrange

# master
set acc_hardware = MPU6050
set acczero_x = 48
set acczero_y = -31
set acczero_z = 12
set accgain_x = 4082
set accgain_y = 4078
set accgain_z = 4078
set align_mag = CW270FLIP
set mag_hardware = HMC5883
set magzero_x = 116
set magzero_y = -54
set magzero_z = -17
set baro_hardware = MS5611
set blackbox_rate_denom = 4
set motor_pwm_rate = 1000
set motor_pwm_protocol = ONESHOT125
set failsafe_procedure = RTH
set nav_extra_arming_safety = OFF
set nav_mc_hover_thr = 1300

# profile
profile 1

set mc_d_pitch = 40
set mc_p_roll = 35
set mc_d_roll = 40
set roll_rate = 40
set pitch_rate = 40

# 

[digitalentity]

This is interesting. Looks like your accelerometer ended up thinging that quad is moving upwards. We might have a bug in accelerometer bias calculation code, but I'm not certain about that.

Can you please do a flight with attached firmware - it's essentially a 1.6.1 + additional logging of a few internal variabled? inav_1.6.1_SPRACINGF3.hex.gz

[krzysztofmatula]

Attaching log covering many RTH actions with the provided firmware:
blackbox_log_2017-04-23_122634.TXT

Unfortunately, the original issue didn't happen, but maybe the additional debugs will show something...

In the meantime I changed the configuration. I'll try to revert to the one I was using yesterday and then I'll do another test flight if only it stops raining (or snowing, whatever it is... I thought it's spring already :| )

# diff

# version
# INAV/SPRACINGF3 1.6.1 Apr 23 2017 / 02:53:31 (6c878ae)

# resources

# mixer

# servo

# servo mix

# feature
feature SOFTSERIAL
feature GPS
feature TELEMETRY
feature RSSI_ADC
feature LED_STRIP
feature PWM_OUTPUT_ENABLE

# beeper
beeper -ON_USB

# map

# serial
serial 0 1 115200 57600 0 115200
serial 1 2 115200 57600 0 115200
serial 2 16 57600 57600 57600 115200
serial 31 1 9600 38400 0 115200

# led
led 0 15,15:S:C:2
led 1 14,15:S:CW:2
led 2 9,15:S:CW:2
led 3 8,15:S:CW:2
led 4 7,15:S:CW:2
led 5 6,15:S:CW:2
led 6 1,15:S:CW:2
led 7 0,15:S:C:2
led 8 11,0:N:C:1
led 9 10,0:N:CW:1
led 10 9,0:N:CW:1
led 11 8,0:N:CW:1
led 12 7,0:N:CW:1
led 13 6,0:N:CW:1
led 14 5,0:N:CW:1
led 15 4,0:N:C:1

# color

# mode_color

# aux
aux 0 2 1 900 1200
aux 1 4 1 900 1075
aux 2 4 1 1250 1350
aux 3 3 0 1100 1250
aux 4 3 0 1350 1475
aux 5 3 0 1600 1750
aux 6 3 0 1900 2100
aux 7 9 0 1250 1500
aux 8 8 0 1500 2100
aux 9 11 1 1500 2100
aux 10 12 2 1700 2100
aux 11 18 1 1500 2100

# adjrange

# rxrange

# master
set looptime = 1000
set gyro_sync = ON
set gyro_sync_denom = 1
set gyro_hardware_lpf = 188HZ
set gyro_lpf_hz = 90
set gyro_notch1_hz = 400
set gyro_notch1_cutoff = 300
set gyro_notch2_hz = 200
set gyro_notch2_cutoff = 100
set acc_hardware = MPU6050
set acczero_x = 48
set acczero_y = -31
set acczero_z = 12
set accgain_x = 4082
set accgain_y = 4078
set accgain_z = 4078
set align_mag = CW270FLIP
set mag_hardware = HMC5883
set magzero_x = 116
set magzero_y = -54
set magzero_z = -17
set baro_hardware = MS5611
set blackbox_rate_denom = 8
set motor_pwm_rate = 2000
set motor_pwm_protocol = ONESHOT125
set failsafe_procedure = RTH
set nav_extra_arming_safety = OFF
set nav_mc_hover_thr = 1300

# profile
profile 1

set mc_p_pitch = 50
set mc_i_pitch = 50
set mc_d_pitch = 22
set mc_i_roll = 40
set mc_d_roll = 20
set dterm_lpf_hz = 30
set dterm_notch_hz = 260
set dterm_notch_cutoff = 160
set roll_rate = 50
set pitch_rate = 50
set yaw_rate = 45

# 

[digitalentity]

Hm... Accelerometer bias estimation does work correctly, but slowly. However at beginning of flight and at the end of flight measured acceleration is different - possibly temperature drift. Seems that we need temperature-compensation.

[krzysztofmatula]

Reverted to yesterday's setup and still unable to reproduce the issue.
See another log with many RTH attempts:
blackbox_log_2017-04-23_144933.TXT

[krzysztofmatula]

Another interesting thing (not sure if related) is an altitude drift that can be seen in both logs and also this video (corresponding to the latest log): https://www.dropbox.com/s/j59rwder63qfp3m/20170423-143706.AVI?dl=0
During the 10 minute long flight the altitude has drifted for ~7m.
I observe this systematically since some time.

I bet it is not related to temperature. On my yesterdays's log (blackbox_log_2017-04-22_174906.TXT) I flown 2 batteries - first in log 2/3 (altitude drift 5.4m), then 3/3 (drift 3m). During the first flight sensor's temperature should get equal to the ambient temperature.
The sensor itself is protected from the Sun and covered with a foam.

[digitalentity]

Is this still relevant?

[krzysztofmatula]

It's hard to say. It happened to me only once and I was unable to reproduce it again neither on the same nor on any newer firmware.
I propose to close the ticket for now...

[digitalentity]

Thanks for prompt reply. Feel free to reopen if issue occurs again.

[giacomo892]

Happened the same to me once.
Now for safety reasons if I fly away I just fly >AT_LEAST altitude and I'm good to go.

[digitalentity]

@giacomo892 on which version? I bevelie we fixed it in 1.7.2 (or development, I can't remember)

[krzysztofmatula]

My observation was on pre-1.7 version.
I was trying hard to reproduce the issue again to find some relations, but I failed to do so.
I was engaging RTH below the "AT_LEAST altitude", above, close to the altitude, etc. Without success - it was always working. So, after all it was not that bad.

[giacomo892]

@digitalentity I was 1.7.1 development as reported in issue 1675. There is even a log if you want to take a look there.

[krzysztofmatula]

It happened again on inav 1.7.3.
Too bad I don't have a log...
My quad turned towards home and stayed at altitude ~19m according to telemetry. I have the return altitude set to 20m.

[digitalentity]

@krzysztofmatula try increasing inav_w_z_baro_p to 0.5 or even to 0.75.

[krzysztofmatula]

I will check it, however it may take another several months to confirm if it helps.
I'm using RTH quite often and normally it always works.

[krzysztofmatula]

Reopening the issue, as it's related to RTH and Failsafe which should be bullet-proof.

[digitalentity]

Ok, moving to 1.9 then - since we won't be able to confirm it for 1.8

[Dronek]

@krzysztofmatula @digitalentity
Can any correlation with set nav_extra_arming_safety = OFF be excluded here?

I recall at least one flight where I forgot to set this feature to ON and RTH behaved weird.

[krzysztofmatula]

@Dronek, what kind of "weirdness" you mention? Looking at the code this flag should only affect arming. But... in software you can never be sure :)
I always fly with this setting OFF.

[stronnag]

I would imagine that set nav_extra_arming_safety = OFF negates any expectation that RTH might work.

[krzysztofmatula]

I have it off, because it happened once that I wanted to fly in such place and such time that GPS was unable to get fix. I was perfectly OK to fly manually, but I couldn't start at all... At that time I could clearly see no sats on telemetry (thing that I always check). It was definitely not the case last weekend.

Anyway, for the sake of verification I'll set in temporarily ON.

[krzysztofmatula]

So, I did a flight with the mentioned settings:
nav_extra_arming_safety = ON
inav_w_z_baro_p = 0.75

My first impression is that alt-hold is even more smooth (or maybe it was because of no wind...)

I did some flying, tried RTH and... problem occurred! The quad got stuck at ~19m not going to fly towards home.

Then I did another test - lowered altitude and engaged RTH again. The quad went up gently up to ~19m (normally it does this relatively quick!) and... got stuck.

Then I moved it above return altitude and tried RTH again. This time RTH was completed and the quad landed. The battery was drained, so I couldn't test more.

Having it down, I replaced battery and tried to reproduce the case once again. I did multiple attempts to engage RTH below or close to return altitude and it always worked...

I should have a log. I'll try to download it later today (will have to find another PC, as it no longer works on my mac...)

[krzysztofmatula]

Attaching log:
blackbox_log_2017-10-17_224028.TXT

See log 2/3 at 7:25 - RTH made a climb from 12.4m to ~19.6m, not reaching the target altitude and got stuck there.
Again at 8:18 - starting from 13.9m the altitude went up to 19.9m - again, without cruising home.
Finally, at 8:35 - starting from 29.6m the quad finally got home.

Is that the problem that it doesn't actually reach the target altitude?

In log 3/3 there are multiple attempts of engaging RTH and always the climb finishes above 20m and then is immediately followed by cruise back home.

[digitalentity]

@krzysztofmatula thanks, will have a look!

[krzysztofmatula]

Update: in the case at 8:18 the baro is 19.9m, but navPos[2] is actually19.55m, thus the 50cm margin is not reached (navTgtPos[2] = 20.06m) and cruise home is not initiated.
Still, navVel[2] is ~25cm/s, while the altitude actually doesn't increase.

[digitalentity]

@krzysztofmatula accelerometer is picking up "fake" acceleration. Possibly mechanical stress on the board. Increasing inav_w_acc_bias to 0.1-0.2 might help

[krzysztofmatula]

Also it could be accelerometer calibration issue. I'll re-do the calibration. Maybe the characteristics changed over time. If I read the log correctly there is always 1.02..1.05g reported when in hover and also on the ground.

[krzysztofmatula]

See this fix for initial climb margin calculation: #2371
This will fix (or rather hide) the issue for me.

I'd go one step further and make the climb->cruise transition more robust. Currently a target altitude is set and the climb is to finish exactly there (minus the margin). This results in climbing speed to decrease as the target altitude approaches, eventually stopping there (or not reaching, as in my case).
I'd prefer not too loose time on this transition (also to conserve battery), go up with the climbing speed set and immediately switch to cruise home, possibly overshooting the altitude slightly at beginning.

What do you think?

[digitalentity]

How about setting initial climb target altitude higher than actual RTH altitude and still use actual RTH altitude to detect that climb has finished?

[krzysztofmatula]

There are several possibilities of implementing this feature. Another one could be increasing the margin. For example having set the RTH altitude at 20m I'd be fine to start the cruise at, say, 16m (while still climbing up to 20m).

In all cases it seems there would be a need to have an altitude window where cruise back home can happen. Allowing this to be configured (another option) could be fine on one hand, but on the other I'm not in favour of overcomplicating things. This is a security feature and risk of having it misconfigured should be minimized.

I think I'll do some experiments to see which option works the best (initially with some hard-coded values).

[oleost]

Is not there already and implemention of this for fixed wing? Just use the same?

I think it was when within 10% of target height start heading home.

[krzysztofmatula]

For FW the implementation was also incorrect.
It was: 10%, at most 1m.
Should be: 10%, at least 1m.

[digitalentity]

@oleost yes, we got that logic wrong when adding

[fiam]

I think we should make the climbing procedure a bit more robust. If we haven't reached the target altitude and we're neither climbing nor descending significantly after, let's say, 15 seconds we should probably head home anyway and try to climb on the way. Once we reach the home point, start either landing or holding position at the altitude we're at (depending on the configuration).

This would prevent the aircraft from climbing indefinitely in case of a bug somewhere else. Note that not reaching the target altitude could cause severe problems in FW, since we can't climb while being stationary. It's definitely better if we put some extra guards in place. Maybe we should also disable "Climb First" in FW by default?

[stronnag]

flight tested 7d01b53adc99525426e9087c4d60ca5af301c199. No regressions compared to prior implementation and the stronnag placebo may even think that RTH was a bit snappier than usual.

[digitalentity]

I think we can close this issue and open a new one to discuss @fiam's suggestion.

[krzysztofmatula]

I wouldn't close this case as the initial problem hasn't been actually solved.
With default RTH altitude (10m) the margin is 50cm, regardless of the MIN/MAX issue being fixed or not. With such margin my quad gets stuck at the climb phase.

My setup probably has accelerometer issues, however as they are normally not noticeable, I'd assume more setups may have them as well. Thus I think making the climb->cruise transition more robust is a must to close the case. I'm going to do some flight tests with the proposed changes within the weekend and let you know the conclusions.

[fiam]

I don't have a strong opinion on this one. We can either open a new issue or continue the work and conversation on this same issue. In any case, I've just committed a proof of concept of how this could look like: https://github.com/iNavFlight/inav/commit/4b2bb275f8651aaed587c76b76567badd03b6d8f. I hardcoded the values to a 10 second timeout while not having more than 10cm/s of vertical speed. Feedback welcome.

[oleost]

@fiam what will happen if it reaches home before finished climbing?

[krzysztofmatula]

So, I did 3 tests and, collected blackbox log, video and fpv video.
See them here:
https://www.dropbox.com/sh/kwz9av43amklh42/AABe_vcmVtm0lebwXan_5Gtfa?dl=0

1. RTH as for today (altitude: 20m, margin calculated: 1m)

Video1 shows that the gap between completing the climb (visually) and starting cruise is ~5s. Also this is the point that it sometimes was getting stuck.

1. RTH with margin hardcoded to 4m:

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index e337a993..532b5972 100755
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -862,7 +862,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
     if (posControl.flags.hasValidPositionSensor || !checkForPositionSensorTimeout() || navConfig()->general.flags.rth_climb_ignore_emerg) {
         const float rthAltitudeMargin = STATE(FIXED_WING) ?
                             MAX(100.0f, 0.10f * ABS(posControl.homeWaypointAbove.pos.V.Z - posControl.homePosition.pos.V.Z)) :  // Airplane: 10% of target altitude but no less than 1m
-                            MAX( 50.0f, 0.05f * ABS(posControl.homeWaypointAbove.pos.V.Z - posControl.homePosition.pos.V.Z));   // Copters:   5% of target altitude but no less than 50cm
+                            MAX(400.0f, 0.05f * ABS(posControl.homeWaypointAbove.pos.V.Z - posControl.homePosition.pos.V.Z));   // Copters:   5% of target altitude but no less than 50cm

         if (((posControl.actualState.pos.V.Z - posControl.homeWaypointAbove.pos.V.Z) > -rthAltitudeMargin) || (!navConfig()->general.flags.rth_climb_first)) {
             // Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance

This looks just perfect for me. The most smooth and robust one!
The only thing (that may be an issue for someone) is that the cruise started 4m below the target altitude (altitude - margin).

1. RTH with target altitude 4m above target home point (only during the climb):

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index e337a993..020e3b2d 100755
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -889,7 +889,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
                     setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
                 }
                 else {
-                    setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
+                    t_fp_vector pos2 = posControl.homeWaypointAbove.pos;
+                    pos2.V.Z += 400;
+                    setDesiredPosition(&pos2, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
                 }
             }

This is also rather robust, but at the transition from climb to cruise the throttle is cut down (because we still have vertical speed while the target altitude is reached). This makes an impression that the machine falls down (hear the "video3").

Conclusion:

What works best for me is to allow to set the "margin" high enough to have the transition smooth and safe. I'm not sure if it should be set by default or manually with yet another option. Or both: option to set and big enough default value.

What do you think?

[giacomo892]

Beside Video 1 everything looks very nice! Quad arrives at right altitude and immediatelly returns home!
Seems like having a "big" margin really help a lot in the smoothness of the maneuver!

OT. What machine is that?

[krzysztofmatula]

@giacomo892, it's a cheap chinese frame (which appeared to be quite durable), ~280mm, customized, ~700g, SPF3.

[krzysztofmatula]

This works quite well for me: #2380

See also the "video4" and "fpv4":
https://www.dropbox.com/sh/kwz9av43amklh42/AABe_vcmVtm0lebwXan_5Gtfa?dl=0

[digitalentity]

@fiam timeout is a good idea, but we need to make it smart enough to actually help.

[krzysztofmatula]

Closing, as the fix works well. Feel free to reopen if improvements are still wanted.