AP_InertialSensor: Correct out-of-bounds array access that was causing SITL to crash