Finding algorithm for path planning in drones in 3D space along with obstacle avoidance.

I have a function for A star pathfinding that does not allow diagonal movement where I have penalties for changing direction (F score increased). However, I have found that the resulting path is not always linked, often jumping back to a random square.

I am trying to code a simple pathfinding algorithm to find the path from a location to an end goal given a list of blocked points (squares). however my code just keeps running even if the end goal is right next to the location. I have attached screenshots of all relevant pieces of my code.