AP_Math: handle NaN in constrain(), returning average

this makes it less likely a NaN will propogate
L1_Control: constrain the roll to handle floating point errors