Il semble que ton drone prenne en compte partiellement la commande de RTH, puisqu'il monte à la hauteur prévue pour le RTH.
Ensuite il ne fait pas le retour, comme si la commande RTH avait disparu. D'où mon hypothèse (loin d'une certitude) d'un souci sur le switch.
Tu peux aussi faire des essais en basculant le switch tout doucement, pour éviter un éventuel rebond au moment de la commutation....