1 files changed, 21 insertions, 11 deletions

diff --git a/main.c b/main.c
index 5925c02..d905a60 100644
--- a/main.c
+++ b/main.c
@@ -53,30 +53,40 @@ enum CTRL {
 
 static volatile enum CTRL cmd = NONE;
 
-static inline void pwron(uint8_t port)
+static inline void pwron_fpm(void)
 {
-	PWR_PORT |= (1 << port);
+	PWR_PORT &= ~(1 << PWR_FPM);
 }
 
-static inline void pwroff(uint8_t port)
+static inline void pwroff_fpm(void)
 {
-	PWR_PORT &= ~(1 << port);
+	PWR_PORT |= (1 << PWR_FPM);
+}
+
+static inline void pwron_servo(void)
+{
+	PWR_PORT |= (1 << PWR_SERVO);
+}
+
+static inline void pwroff_servo(void)
+{
+	PWR_PORT &= ~(1 << PWR_SERVO);
 }
 
 static inline void lock(void)
 {
-	pwron(PWR_SERVO);
+	pwron_servo();
 	OCR1A = PWM_MID;
 	_delay_ms(500);
-	pwroff(PWR_SERVO);
+	pwroff_servo();
 }
 
 static inline void unlock(void)
 {
-	pwron(PWR_SERVO);
+	pwron_servo();
 	OCR1A = PWM_MAX;
 	_delay_ms(500);
-	pwroff(PWR_SERVO);
+	pwroff_servo();
 }
 
 static inline void flash_led(void)
@@ -102,7 +112,7 @@ int main(void)
 
 	uart_init();
 	PWR_DDR |= (1 << PWR_FPM) | (1 << PWR_SERVO);
-	pwron(PWR_FPM);
+	pwron_fpm();
 	fpm_init();
 
 	/* servo */
@@ -174,7 +184,7 @@ int main(void)
 		cmd = NONE;
 		_delay_ms(500);
 
-		pwroff(PWR_FPM);
+		pwroff_fpm();
 		set_sleep_mode(SLEEP_MODE_PWR_DOWN);
 		sleep_enable();
 		sleep_bod_disable();
@@ -183,7 +193,7 @@ int main(void)
 
 		cli();
 		sleep_disable();
-		pwron(PWR_FPM);
+		pwron_fpm();
 		fpm_init();
 	}