Invalid setpoint yaw after takeoff

[imrhch]

https://github.com/PX4/Firmware/blob/master/src/modules/navigator/mission_block.cpp#L528

I believe sp.yaw_valid should be set correctly in the MissionBlock::mission_item_to_position_setpoint according to (mission) item->yaw.
When yaw_valid is already true (in my case it was set when taking off using MAV_CMD_NAV_TAKEOFF) and mission_item_to_position_setpoint is called with mission item containing yaw=NAN (is happens whan take-off is finished: https://github.com/PX4/Firmware/blob/master/src/modules/navigator/takeoff.cpp#L95), the yaw remains set to NAN and the yaw_valid remains true. When I switch to offboard (without valid yaw set), NAN value from yaw propagates thru the control and causes invalidating attitude setpoint (most values are set to NAN). This leads to instant crash.

[imrhch]

It seems that adding line
sp->yaw_valid = !isnanf(item->yaw);
after this one
https://github.com/PX4/Firmware/blob/master/src/modules/navigator/mission_block.cpp#L528
solves the problem.

Without changing the firmware, I was able to overcome the problem by sending few SET_POSITION_TARGET_LOCAL_NED messages with yaw set just after switching to offboard mode and return to sending yaw_rate only after short time.

[sanderux]

Could you add a pr on this?

[Seanmatthews]

@imrhch @sanderux Created the pull request to get this out of the way, and loosely deals with some issues on which I've been working.