Multirotor course hold/cruise mode

[breadoven]

Adds course hold/cruise mode for multirotors.

Speed is controlled using the pitch stick. Heading is controlled using the yaw stick or the roll stick with the rate of heading change set by nav_cruise_yaw_rate (same setting as nav_fw_cruise_yaw_rate which is now redundant). Altitude is controlled as normal using throttle. Movement is currently limited to the forward direction only.

Raising the pitch stick sets the speed during cruise, with speed set in proportion to the deflection up to a maximum of nav_manual_speed. This speed is maintained after the stick returns to centre. To slow down and stop the pitch stick is lowered with speed of slowing proportional to stick deflection. Maximum deflection should take around 2s to slow to a stop with position hold active when the speed drops below 0.5m/s.

OSD system message provides indication of selected speed or if Hold is active.

processNavigationRCAdjustments and applyMulticopterPositionController have also been refactored/cleaned up as part of this change.

Seems to work as expected from preliminary testing with ground course tracking reasonably close to the set cruise course even in decent cross winds. More testing still required though to be sure.

[Jetrell]

I have a few questions before testing.
Can I assume that CRUISE mode is based on nav_user_control_mode = Attitude. Rather than nav_user_control_mode = Cruise which transforms the stick command to estimated speed and position ? As occurs in POSHOLD when a stick command is feed-in.

Raising the pitch stick sets the speed during cruise, with speed set in proportion to the deflection up to a maximum of nav_manual_speed.

Is this also constrained by nav_mc_bank_angle in CRUISE mode ?
Otherwise, how does it balance between forward velocity and altitude control, when navigation auto throttle is in operation, without it maxing out the throttle at 100%, if the user commands a pitch bank angle > 45 degrees?

[breadoven]

I have a few questions before testing. Can I assume that CRUISE mode is based on nav_user_control_mode = Attitude. Rather than nav_user_control_mode = Cruise which transforms the stick command to estimated speed and position ? As occurs in POSHOLD when a stick command is feed-in.

It bypasses the ATTI setting if set so it's always in CRUISE essentially. And this change doesn't set any position it just directly sets the required velocity in the forward direction based on the desired speed defined by the pitch stick deflection, a speed that is held after the stick is centered. Position is only used to hold the position if the desired speed drops below 0.5 m/s.

Is this also constrained by nav_mc_bank_angle in CRUISE mode ? Otherwise, how does it balance between forward velocity and altitude control, when navigation auto throttle is in operation, without it maxing out the throttle at 100%, if the user commands a pitch bank angle > 45 degrees?

It will work with the same constraints as POSHOLD CRUISE including nav_mc_bank_angle so if that works correctly this should do also. This is much the same as POSHOLD CRUISE except you don't need to keep the pitch/roll stick deflected to maintain speed and it only flies in the forward direction. Also you can only change direction by changing heading but I guess that's how POSHOLD CRUISE works if you only want to fly forward. The other difference is changing heading is limited to the rate set by nav_cruise_yaw_rate . This seems a more controlled way of changing course than allowing the normal free yaw control. Not sure if 60 degs/sec is right for a multirotor, might be better to increase that limit, perhaps up to 90.

[DzikuVx]

I can't wait to test it!

[Jetrell]

@breadoven Both Cruise and Course hold perform rather nicely in the most part.

Although I picked up on a few issues.

• When the pitch stick is pushed forward to start forward travel.. If the pitch angle is anything less than nav_mc_bank_angle is set too. The quad will continually bob up and down. Just like it would when try to stay below the nav_manual_speed or nav_auto_speed value.. But in both cases today on two different quads.. The actual speed was not getting near the nav_manual_speed set point. Which I always have set to its max. 72km/h. So it appears to be ignoring nav_manual_speed, and only taking notice of the pitch stick value you push the nav_mc_bank_angle to.
• When it comes to pulling the pitch stick back, to slow or bring the MC to a stop.. It maybe an idea to have a quick reference of when its below 0.5m/s.. Otherwise, its a bit fiddly to be watching the pitch angle indicator and the GPS speed, to be certain that its actually stopped.. I often found it was still slowly creeping forwards, when it thought it was just stopped in a hover.
• Like you mentioned above.. I found nav_cruise_yaw_rate = 60 a bit slow in making turn for my liking.. Maybe have the ability to take it to 120dps.

Other test like switch to POSHOLD or RTH while in Cruise or Course Hold modes worked fine.

[breadoven]

• When the pitch stick is pushed forward to start forward travel.. If the pitch angle is anything less than nav_mc_bank_angle is set too. The quad will continually bob up and down. Just like it would when try to stay below the nav_manual_speed or nav_auto_speed value.. But in both cases today on two different quads.. The actual speed was not getting near the nav_manual_speed set point. Which I always have set to its max. 72km/h. So it appears to be ignoring nav_manual_speed, and only taking notice of the pitch stick value you push the nav_mc_bank_angle to.

The speed set uses the same method used to set the speed for Poshold in the CRUISE setting, just a function of stick position, deadband and limiting speed so I'm not sure why it wouldn't achieve the same speed as Poshold mode. Unless there's some ATTI related limit I've missed. It shouldn't base the speed on bank angles related to pitch stick position. A log would be useful. Check desired velocity 0 and 1 to know what target speed has been set (square the velocities, add together and take the root to get the overall speed).

• When it comes to pulling the pitch stick back, to slow or bring the MC to a stop.. It maybe an idea to have a quick reference of when its below 0.5m/s.. Otherwise, its a bit fiddly to be watching the pitch angle indicator and the GPS speed, to be certain that its actually stopped.. I often found it was still slowly creeping forwards, when it thought it was just stopped in a hover.
• Like you mentioned above.. I found nav_cruise_yaw_rate = 60 a bit slow in making turn for my liking.. Maybe have the ability to take it to 120dps.

I was wondering if any feedback made sense such as a system message showing the current set speed or if hold is active. The best way of being sure it's dropped below 0.5m/s is to just hold the stick at full down deflection for several seconds just remembering that it won't hold until you release the stick back to center.

[DzikuVx]

@breadoven I tested it today and I have to say, if it activates then it works great!
However in around 50% of cases Cruise fails to activate and causes spin of death that can be unrecoverable!
Here is the DVR

[DzikuVx]

https://youtu.be/5cxcgukc-vs

[DzikuVx]

LOG00003.TXT
Promised log of the event

[breadoven]

Ah, that's not good. Looking at the log (time 25.520) it seems to be the same problem as the Emergency Landing crashes. The filtered gyro values seem to be nonsense compared to the raw gyro values. Hard to see how this is Nav related ... surely more to do with a basic stability, gyro filter issue ? I did find a bug in the altitude control code as mentioned here #9184 (comment). Do you think setting the throttle to 0 rather than hover throttle when the altitude controller is reset could randomly trigger some odd behaviour in the gyro filtering ? You'd think it would affect all the Nav modes though which doesn't appear to be the case. Very strange.

[DzikuVx]

I don't think it's gyro filtering issue. I think it's a race condition that happens during enabling of attitude control. Not sure how it's yet possible thought. Acro position hold, rth etc all work fine Wysłano z programu Outlook dla systemu Android<https://aka.ms/AAb9ysg>

…

________________________________ From: breadoven ***@***.***> Sent: Thursday, August 10, 2023 8:58:10 PM To: iNavFlight/inav ***@***.***> Cc: Paweł Spychalski ***@***.***>; Comment ***@***.***> Subject: Re: [iNavFlight/inav] Multirotor course hold/cruise mode (PR #9213) Ah, that's not good. Looking at the log (time 25.520) it seems to be the same problem as the Emergency Landing crashes. The filtered gyro values seem to be nonsense compared to the raw gyro values. Hard to see how this is Nav related ... surely more to do with a basic stability, gyro filter issue ? I did find a bug in the altitude control code as mentioned here #9184 (comment)<#9184 (comment)>. Do you think setting the throttle to 0 rather than hover throttle when the altitude controller is reset could randomly trigger some odd behaviour in the gyro filtering ? You'd think it would affect all the Nav modes though which doesn't appear to be the case. Very strange. — Reply to this email directly, view it on GitHub<#9213 (comment)>, or unsubscribe<https://github.com/notifications/unsubscribe-auth/AAHMBG5TYBUDN33ON6I3TC3XUUVMFANCNFSM6AAAAAA3DK5I4M>. You are receiving this because you commented.Message ID: ***@***.***>

[DzikuVx]

I did some check trying to understand the issue and I think what we see in gyro traces is the effect, not the cause.

See here:

Even begins on roll, then follows pitch, yaw and only finally filtered gyro starts to report crap.
I will try to run some extra tests today, especially to verify if PosHold entry does not cause such issues

[breadoven]

Looking at it again it does seem to be the case that the motors drop pretty much to 0 when the throttle first drops to 0 but then 3 and 4 rapidly speed up with 1 and 2 still almost at 0 which would cause a roll. I think I'll merge (#9169) since it will stop the throttle dropping to 0 when the altitude controller gets reset, something that shouldn't happen.

The other thing that's notable from your log @DzikuVx is that from 14s onward navPos[2] and vertical velocity look completely unrealistic, peaking at -100m and -54m/s, whilst Baro only varies between about 6 and 22m.

[DzikuVx]

@breadoven on the topic of altitude, it was caused by sudden g forces that messed up altitude estimation

[DzikuVx]

Now, this is how successful entry into Cruise looks like:

The broken one looks like this:

The rcCommand overriden by NAV engine is completely bonkers! Because NAV engine works only with positions and attitudes it rather does not take it into computation.
I think there is a race condition in either CRUISE initialization or position controller that causes .

Demanded speed increases from 1.3m/s to 3,9m/s instantly

[breadoven]

The other thing that happens is the gyro appears to be inverted again, as happened with the first Emergency Landing crash. At 25.521 the raw and filtered gyro roll is -ve whereas it's obvious from the DVR that the quad rolled right which should be +ve gyro should it not. Seemed to be correct earlier in the flight, e.g. at 5.683 in ACRO with left roll stick giving -ve rcCommand with -ve gyro values and at 8.500 right roll stick results in +ve gyro values. Something's getting a bit confused somewhere.

[DzikuVx]

@breadoven I run more test trying to isolate the cause. So, I concentrated on transitions from and to different modes.

1. Acro -> Angle transition -> No problem detected
2. Acto -> PosHold transition -> No problem detected
3. Angle -> PosHold transition -> No problems detected
4. Acro -> Cruise transition -> Spin of death is 70% of attempts
5. Angle -> Cruise transition -> Spin of death

But what's most interesting I managed to make Cruise work reliably. You only have to:

1. Engage Position Hold
2. Engage Cruise -> PosHold takes priority, Cruise not engaged
3. Disable PosHold -> Cruise engages after PosHold was enabled

In this configuration Cruise was engaging all the time without spin of death.

So I really think that navOnEnteringState_NAV_STATE_CRUISE_INITIALIZE and navOnEnteringState_NAV_STATE_CRUISE_IN_PROGRESS are missing something important that navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE and navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS do.

How about calling navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE in navOnEnteringState_NAV_STATE_CRUISE_INITIALIZE?

[DzikuVx]

Strange gyro traces are a result or gyro saturation, not a cause

[DzikuVx]

Oh, and finally, I won't be able to test anything for a few days :) Test quad got damaged in the process

[breadoven]

OK, I'll have a look at it again, see if I can work out what's missing in the initialisation. Should be the same issue for Emergency Landing although I don't think anything's really changed in the way that's initialised but then maybe it never really got tested enough to find the problem.

Did you try fixing the throttle when the altitude controller is reset, setting it to Hover throttle rather than 0 ? The fact it works reliably from Poshold might be due to the fact the altitude controller doesn't reset because the last update will still be within the timeout period avoiding the throttle "glitch".

Sounds like a crash test quad is required ... something with some sort of large polystyrene protective cover with prop guards. Multirotors are too unforgiving otherwise if things go wrong.

[DzikuVx]

It was the same code as before, no changes at all

[breadoven]

I rechecked the code and with the exception of a couple of things there isn't any obvious difference between initialisation of Poshold and Cruise. One exception is NAV_REQUIRE_THRTILT which should be included in the Cruise section. The other exception is the reference to FLIGHT_MODE(NAV_COURSE_HOLD_MODE) at a point in rcAdjustment where the mode flag may not have been set before the function is called (only happens for first loop after switching mode on).

However, I can't see the above things making much difference and causing the loss of control. The main reason for this is Nav is not directly controlling attitude when the instability starts, the Nav controllers don't start controlling until 20ms after the Nav mode is initialised by which time the quad is already tumbling (this is the period where the throttle is set to 0 with the throttle only increasing when the Nav altitude controller first starts controlling). The only thing Nav does do up to that point is reset the Nav controllers and switch on Angle (set for Poshold and Cruise). Resetting the Nav controller sets the throttle to 0 (a bug, now fixed with throttle correctly set to hover throttle). So the only thing actively controlling the motors in this 20ms window is Angle control. I noticed the successful Cruise initiation started with roll and pitch angles close to 0, the second initiation with the recovered loss of control started with roll and pitch angles around 8 degrees but the final initiation, resulting in the crash, started with roll of 60 and pitch of 25 or so degrees (although it looked level in the DVR ?). So this looks more like an Angle control issue although why it happens in Cruise (and Emergency Landing ?) and not in Poshold I've no idea unless it just depends on the attitude conditions on initialisation. Can't help but think setting the throttle to 0 rather than hover on reset really can't have helped here.

Edit: Or has Angle been set when the mode is initialised ? May not actually be set until the next Rx update. Needs checking. Might have still have been in ACRO in that case during the 20ms. (after checking it seems Angle would have activated 4ms after the mode was selected so was controlling when the loss of control happened).

[DzikuVx]

time for more tests

[Jetrell]

@breadoven I think I stumbled across the reason Poshold and now MC Cruise mode in nav_user_control_mode = Cruise performs this "bobbing" over speed slowdown effect.
Its rather noticeable with the SPD target message you now have on the OSD...
I now see that nav_manual_speed derives that speed value from an estimated target speed. And not directly from the GPS CoG speed.

Note my settings.
nav_mc_bank_angle = 28
nav_manual_speed = 72km (max allowed)

You can see how the quad is bobbing up and down in the video. But look right at the end. And compare the target speed when it hits 72km/h, with the GPS 3D speed, being only at 42Km/h.. At a guess. I'd say the inaccurate estimated speed is often bouncing off nav_manual_speed setpoint, when the pitch tilt angle is below nav_mc_bank_angle ?

slow.down.issue.mp4

[DzikuVx]

I tested this today and looks like it's ready to merge :)

[DzikuVx]

@breadoven could you please resolve conflicts? Thanks!

[Jetrell]

I tested this today and looks like it's ready to merge :)

@DzikuVx What about the above video I posted ? Did you experience this issue too, when your multicopter was cruising under similar conditions ?
I've had this occur every time, with three different quads of various sizes.

[breadoven]

I tested this today and looks like it's ready to merge :)

OK, good to hear that the gyro death spin issue is probably fixed.

@DzikuVx What about the above video I posted ? Did you experience this issue too, when your multicopter was cruising under similar conditions ? I've had this occur every time, with three different quads of various sizes.

Surely the quad in the video is hitting the pitch limit causing it to back off before speeding up again. Problem is it's not very subtle in the way it does it causing a lurching motion. It over accelerates too abruptly before hitting either the speed or bank limit, then abruptly backs off causing it to slow down to much, followed by abrupt acceleration again. Eventually it usually settles down but its looks pretty wasteful energy wise. I noticed this a while ago and tried to improve it by damping down the acceleration as it approached the target speed. It sort of worked but I didn't do any more with it. Maybe worth looking at it again ...

[DzikuVx]

I have not had such issues.

[DzikuVx]

OK, NVM, I resolved conflicts and will merge as soon as pipeline finishes

[breadoven]

OK, NVM, I resolved conflicts and will merge as soon as pipeline finishes

Ah thanks for that, sorry for not fixing it sooner. Unfortunately you brought the dreaded hard tabs back in, seems they don't want to go without a fight. No matter ... next change on navigation.c will see them gone.

[Jetrell]

Surely the quad in the video is hitting the pitch limit causing it to back off before speeding up again.

This will happen at any pitch angle below nav_mc_bank_angle as well..
And if I push the stick forward enough, So that nav_mc_bank_angle is thoroughly meet. This effect never occurs.

This issue also exists in Poshold and WP missions as well. And is consistently related to nav_manual_speed being exceeded. But as you could see in that video. It was far from being exceeded (CoG speed)..
What is the source you derived the SPD message target from ? I wonder why its estimation all over the place ? I've looked at many videos and logs. And its estimation is inconsistent with real time events. (pitch angle, throttle and actual wind direction and speed at the time of testing.)

I understand this issues is not caused by the Cruise feature.. But this feature certainly makes the issue stand out more than before.. If I've experienced it with three different size quads, then its highly likely to be reported by someone else as a bug once released.

[breadoven]

This issue also exists in Poshold and WP missions as well. And is consistently related to nav_manual_speed being exceeded. But as you could see in that video. It was far from being exceeded (CoG speed).. What is the source you derived the SPD message target from ? I wonder why its estimation all over the place ? I've looked at many videos and logs. And its estimation is inconsistent with real time events. (pitch angle, throttle and actual wind direction and speed at the time of testing.)

The SPD message is simply the target cruise speed based on the pitch stick demand and nav_manual_speed. Full pitch stick deflection sets the target speed to nav_manual_speed, a 50% deflection would give you half of nav_manual_speed etc. It has nothing to do with actual estimated speed.

I understand this issues is not caused by the Cruise feature.. But this feature certainly makes the issue stand out more than before.. If I've experienced it with three different size quads, then its highly likely to be reported by someone else as a bug once released.

I noticed it mainly during WP missions although it only happens during the initial acceleration after a WP. It always settled down after a few overspeed/over braking speed oscillations. Playing with PIDS didn't seem to help much.

[Jetrell]

The SPD message is simply the target cruise speed based on the pitch stick demand and nav_manual_speed. Full pitch stick deflection sets the target speed to nav_manual_speed, a 50% deflection would give you half of nav_manual_speed etc. It has nothing to do with actual estimated speed.

Having SPD set by percentage of stick deflection and nav_manual_speed, would surely exaggerate the prevalence of it occurring when the user has a higher value set in nav_mc_bank_angle.. Because a higher bank angle will make the MC accelerate faster, than if it was set to a lower value.
I would have thought that actual CoG speed would make more sense to be the target. After all, it is point A to point B we are gauging.

[breadoven]

The SPD message is simply the target cruise speed based on the pitch stick demand and nav_manual_speed. Full pitch stick deflection sets the target speed to nav_manual_speed, a 50% deflection would give you half of nav_manual_speed etc. It has nothing to do with actual estimated speed.

Having SPD set by percentage of stick deflection and nav_manual_speed, would surely exaggerate the prevalence of it occurring when the user has a higher value set in nav_mc_bank_angle.. Because a higher bank angle will make the MC accelerate faster, than if it was set to a lower value. I would have thought that actual CoG speed would make more sense to be the target. After all, it is point A to point B we are gauging.

The speed is always GPS CoG speed unless a pitot is fitted. The acceleration controller should try and match the acceleration to the velocity error so the bank angle reduces as you near the target speed, ideally avoiding overshoot. I got the impression the reason you do get overshoot is possibly down to the fact some quads are over powered so throttle control vs attitude speed control becomes more tricky. That was what seemed to be happening when I played with it last time. I added a setting that attenuated the acceleration because as far as I could see the acceleration controller simply allowed too much acceleration for too long leading to overspeed. The setting cut back the acceleration rapidly as you neared target speed. It seemed to help but not entirely.

[rts18]

If I've experienced it with three different size quads, then its highly likely to be reported by someone else as a bug once released.

I totally agree with your assessment.

Having SPD set by percentage of stick deflection and nav_manual_speed, would surely exaggerate the prevalence of it occurring when the user has a higher value set in nav_mc_bank_angle.. Because a higher bank angle will make the MC accelerate faster, than if it was set to a lower value.

My testing of this feature has shown this to be a big problem as well.
Bank angle plays a greater roll in controlling forward velocity than any other setting.
If I limit nav_mc_bank_angle to a measly 22°. Or detune the velocity controller. The hiccup goes away. But what good is 22° of pitch bank angle in a headwind, if you require the use of RTH? Or why detune the velocity controller. To make other navigation modes perform substandard.

I added a setting that attenuated the acceleration because as far as I could see the acceleration controller simply allowed too much acceleration for too long leading to overspeed. The setting cut back the acceleration rapidly as you neared target speed. It seemed to help but not entirely.

@breadoven Can you attenuate the acceleration controller more. According to the set value of nav_mc_bank_angle?
Values greater than 25° noticeably increase acceleration and forward speed.

[breadoven]

@breadoven Can you attenuate the acceleration controller more. According to the set value of nav_mc_bank_angle? Values greater than 25° noticeably increase acceleration and forward speed.

This isn't part of this change just something I did a while ago for myself to improve behaviour during WP missions. It never made it to a PR.

[rts18]

This isn't part of this change just something I did a while ago for myself to improve behaviour during WP missions. It never made it to a PR.

Any chance of adding that improvement to this feature?

[breadoven]

This isn't part of this change just something I did a while ago for myself to improve behaviour during WP missions. It never made it to a PR.

Any chance of adding that improvement to this feature?

Not sure it was really the right way of fixing this issue, more an experimental bodge to see what could be done. Have you tried playing around with the PID settings ? I tried that with limited success using extreme values that helped in one way but made something else worse. Needs looking at again because it should be possible to limit the over acceleration in some way or another as the target speed is reached.

[rts18]

Have you tried playing around with the PID settings ? I tried that with limited success using extreme values that helped in one way but made something else worse.

I messed around with the nav_mc_pos_xy pids.
I had the same success as you did by the sound of it.
It helped cruise mode. But caused target overshoot in navigation modes.

Needs looking at again because it should be possible to limit the over acceleration in some way or another as the target speed is reached.

It would be the icing on the cake.
A quad with hiccups, takes the shine of an otherwise perfect feature.

[P-I-Engineer]

i've been trying to test this, i've flashed master (ad8....) when i switch to cruise mode, nothing happens. it shows acro in the osd. what am i missing? cruise_yaw is at default of 20.

[breadoven]

i've been trying to test this, i've flashed master (ad8....) when i switch to cruise mode, nothing happens. it shows acro in the osd. what am i missing? cruise_yaw is at default of 20.

It was broken by #8555. I did leave comments to highlight the problem but no fix to date it seems. @shota3527 ?