Systems of today are becoming more complex; they have many levels of the control hierarchy, are software-intensive, use different networks, have increasing processing power, use a diversity of devices, and require more integration. Systems-Theoretic Process Analysis (STPA) is a technique that is being used to analyze the safety of those systems at the concept stage. For the design phase, STPA can be combined with SysML modeling activities, including simulation and formal verification of systems models to produce the control software more efficiently. However, for the design phase, when starting from the STPA analysis there is no support to elaborate the control algorithm. Building the control algorithm is one of the most difficult tasks in the design phase. We propose a method to synthesize the control algorithm for safety-critical systems from the STPA analyses and the functional requirements. Our method maps the control structure (STPA) into a block diagram (SysML), and it uses the STPA results to generate an initial state machine diagram (SysML) for automated controllers, actuators, and sensors. We use our method to generate the control algorithms for an Adaptive Cruise Control system. We evaluate the synthesized algorithms by performing model simulation and formal verification. This illustrates that our method is a systematic way to synthesize control algorithms that satisfy both safety and functional requirements.