Px4-autopilot: [Navigator] Fly away because of NaNs

[bresch]

Last day I had a fly away with my VTOL doing a mission. Fortunately, I was able to take it over the RC and came back in manual.
Inspection of the log revealed that the L1 controller was outputting NaNs for roll and yaw setpoints.

Here is the log: http://logs.px4.io/plot_app?log=d72b30cd-d713-4c43-9f32-30510657f2e6

And here is a dump of the setpoints getting into a NaN state:

[LorenzMeier]

@bresch that was not the L1 controller. I can almost guarantee that it was fed NaN setpoints. @Stifael can you pitch in?

[dagar]

Yes, so far I'm seeing a NaN in current.alt when it starts.

[dagar]

At ~479s - end the previous lat/lon are NaN, but valid is true.

[dagar]

So ~479s is when the setpoint type switched from position to loiter. Previous shouldn't be marked valid, but FW loiter isn't using it.

The estimated position and current lat/lon all seem fine during the transition. The alt setpoint is NaN, but that isn't used to calculate the roll and yaw setpoints.

https://github.com/PX4/Firmware/blob/master/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp#L769

[bresch]

Actually, the loiter worked. It went wrong just after that

[dagar]

My bad, I flipped the colors. It's the end of the loiter back to position where it happens.

tl;dr the problem is the previous position setpoint lat/lon are NaN, but it's marked valid. @Stifael

[dagar]

The problem is the LOITER exit here - https://github.com/PX4/Firmware/blob/master/src/modules/navigator/mission_block.cpp#L374

The next position setpoint is the same as the current, which means the distance is 0 and the inner_angle becomes NaN.

Fix

1. Check the range here - https://github.com/PX4/Firmware/blob/master/src/modules/navigator/mission_block.cpp#L376
2. Populate position setpoint next

@NaterGator FYI

Thanks to the original mission file from @bresch I was able to reproduce exactly in SITL.
http://logs.px4.io/plot_app?log=7b4aba61-9165-424f-9578-6779e8f66d98

@bkueng what do you think about dumping the original dataman mission items to logger? Then adding something to pyulog to output a .mission file? I know they're already on the sdcard, but I can think of many occasions where this would have been extremely helpful.

[bkueng]

@bkueng what do you think about dumping the original dataman mission items to logger? Then adding something to pyulog to output a .mission file? I know they're already on the sdcard, but I can think of many occasions where this would have been extremely helpful.

Interesting. I agree this can be helpful. I see some difficulties with an actual implementation though:

• reading from dataman (SD card) will block, which can lead to logging dropouts. It can be reduced by splitting the reads over multiple loop iterations, but might still be an issue, and can take a while for larger missions.
• a mission can be updated in-air, so there could be multiple missions in one log.
• Then we would also need some standardized format so that changes to the dataman structs won't break the parsing in pyulog.

[NaterGator]

Sorry just catching up on this after getting back from Canada. Out of curiosity what was in the mission file? Was there intent to have a tangent crosstrack exit from the loiter or was that also a bug?

[dagar]

It was just a regular timed loiter midmission. I believe the loiter was added in QGC without changing any defaults.

[ToppingXu]

I had a same flight issue, but the code is with this fixing. Is there any further fixing about this issue?

[julianoes]

@ToppingXu please create a new issue and add a logfile.

[ToppingXu]

@julianoes now I have found the root reason.
https://github.com/PX4/Firmware/blob/master/src/modules/navigator/mission_block.cpp#L390
The code above will return a nan inner angle when the loitor radius is bigger than the range.

[julianoes]

@ToppingXu good catch! Do you have an idea how it could be fixed? :thinking:

[julianoes]

@ToppingXu I've made a suggestion in #13138.

[ToppingXu]

@julianoes good job. one more question, I am not sure whether below issue happen.
float a = 1.0f;
float b = a;
if (b > 1.0f) {
is there any possible code run here due to the float precise
}

[julianoes]

@ToppingXu I would assume that 1.0f > 1.0f is never true for floating point. I don't see how it would be a different 1.0f :smile:.