Update for 6 Axis

Looks good, but more testing is required. MEGA only