priority_restoration_fix:Review feedback
priority_restoration_fix:Review feedback

  Additional backport of c2c226be1db53dd0c1315e13bbd76ace6538eedf
  sem_holder:Clean up from Review

krbeverx/Firmware,mcgill-robotics/Firmware,PX4/Firmware,jlecoeur/Firmware,dagar/Firmware,acfloria/Firmware,Aerotenna/Firmware,acfloria/Firmware,dagar/Firmware,Aerotenna/Firmware,jlecoeur/Firmware,Aerotenna/Firmware,PX4/Firmware,krbeverx/Firmware,mje-nz/PX4-Firmware,dagar/Firmware,krbeverx/Firmware,mje-nz/PX4-Firmware,dagar/Firmware,krbeverx/Firmware,dagar/Firmware,krbeverx/Firmware,PX4/Firmware,Aerotenna/Firmware,mcgill-robotics/Firmware,jlecoeur/Firmware,PX4/Firmware,krbeverx/Firmware,jlecoeur/Firmware,mcgill-robotics/Firmware,PX4/Firmware,jlecoeur/Firmware,mje-nz/PX4-Firmware,dagar/Firmware,jlecoeur/Firmware,acfloria/Firmware,mcgill-robotics/Firmware,mcgill-robotics/Firmware,Aerotenna/Firmware,dagar/Firmware,jlecoeur/Firmware,Aerotenna/Firmware,acfloria/Firmware,acfloria/Firmware,mje-nz/PX4-Firmware,mje-nz/PX4-Firmware,jlecoeur/Firmware,PX4/Firmware,krbeverx/Firmware,Aerotenna/Firmware,mcgill-robotics/Firmware,acfloria/Firmware,acfloria/Firmware,mcgill-robotics/Firmware,mje-nz/PX4-Firmware,PX4/Firmware,mje-nz/PX4-Firmware