removed debug printf & more filtering
final firmware for 1st stabilized flight