Advanced control of heat pumps with thermal storage and photovoltaics has recently been promoted as a promising solution to help decarbonise the residential sector. Heat pumps and thermal storage offer a valuable flexibilisation mean to integrate stochastic renewable energy sources into the electricity grid. Heat pump energy conversion is nonlinear, leading to a challenging nonlinear optimisation problem. However, issues like global optimum uncertainty and the time-consuming methods of current nonlinear programming solvers draw researchers to linearise heat pump models that are then implemented in faster and globally convergent linear programming solvers. Nevertheless, these linearisations generate some inaccuracies, especially in the calculation of the heat pump''s coefficient of performance (COP). In order to solve all of these issues, this paper presents a heuristic control algorithm (HCA) to provide a fast, accurate and near -optimal solution to the original nonlinear optimisation problem for a single-family house with a photovoltaic system, using real consumption data from a typical Swiss house. Results highlight that the HCA solves this optimisation problem up to 1000 times faster, yielding an operation that is up to 49% cheaper and self -consumption rates that are 5% greater than other nonlinear solvers. Comparing the performance of the HCA and the linear solver intlinprog, it is shown that the HCA provides more accurate heat pump control with an increase of up to 9% in system Operating Expense OPEX and a decrease of 8% in self -consumption values.