Deployment of medical devices on human body in unsupervised environment makes their operation safety critical. Software errors such as unbounded memory access or unreachable critical alarms can cause life threatening consequences in these medical cyber-physical systems (MCPSes), where software in medical devices monitor and control human physiology. Further, implementation of complex control strategy in inherently resource constrained medical devices require careful evaluation of runtime characteristics of the software. Such stringent requirements causes errors in manual implementation, which can be only detected by static analysis tools possibly inflicting high cost of redesigning. To avoid such inefficiencies this paper proposes an automatic code generator with assurance on safety from errors such as out-of-bound memory access, unreachable code, and race conditions. The proposed code generator was evaluated against manually written code of a software benchmark for sensors BSNBench in terms of possible optimizations using conditional X propagation. The generated code was found to be 9.3% more optimized than BSNBench code. The generated code was also tested using static analysis tool, Frama-c, and showed no errors.