How to get this to work with lower notes?

This code works great for notes in the upper register!  Unfortunately, it doesn't seem to work well with lower notes. For example, often times mistakes a C3 for a C4..etc. Any advice/solutions on how to improve this? Anyway to gate/ignore upper harmonics once a fundamental frequency is detected?