AHRS_DCM: sanity check AHRS_RP_P and AHRS_YAW_P
AC_WPNav: bug fix sanity check of set_speed_xy

This corrects a bug that allowed the waypoint speed to be set to zero