Create bsp.c

Contains working code up to state 3 (tuning state)