Ftm: priority queue test successful, added new capacity register
FTM: fixed bug in auto_pop and push comparators
Heap: Bugfix:  downward pathfinding didn't always detect reaching the end of the heap
Merge branch 'proposed_master' of github.com:stefanrauch/bel_projects into proposed_master