diff --git a/lowcar/devices/Device/Device.cpp b/lowcar/devices/Device/Device.cpp
index 396dad37..9259fa26 100644
--- a/lowcar/devices/Device/Device.cpp
+++ b/lowcar/devices/Device/Device.cpp
@@ -63,10 +63,11 @@ void Device::loop() {
     if (this->enabled && (this->timeout > 0) && (this->curr_time - this->last_received_ping_time >= this->timeout)) {
         device_reset();
         this->enabled = FALSE;
-        // Send RST message
-        this->curr_msg->message_id = MessageID::RST;
-        this->curr_msg->payload_length = 0;
-        memset(this->curr_msg->payload, 0, MAX_PAYLOAD_SIZE);
+
+        //Send RST message
+        this->curr_msg.message_id = MessageID::RST;
+        this->curr_msg.payload_length = 0;
+        memset(this->curr_msg.payload, 0, MAX_PAYLOAD_SIZE);
         this->msngr->send_message(MessageID::RST, &(this->curr_msg));
     }
 
diff --git a/lowcar/devices/ServoControl/ServoControl.cpp b/lowcar/devices/ServoControl/ServoControl.cpp
index 4fa72d6f..5111d09e 100644
--- a/lowcar/devices/ServoControl/ServoControl.cpp
+++ b/lowcar/devices/ServoControl/ServoControl.cpp
@@ -42,14 +42,11 @@ size_t ServoControl::device_write(uint8_t param, uint8_t* data_buf) {
             servos[param].attach(this->pins[param]);
         }
         this->positions[param] = value;
-        servos[param].writeMicroseconds(ServoControl::SERVO_CENTER + (this->positions[param] * ServoControl::SERVO_RANGE / 2.0));
+        servos[param].writeMicroseconds(ServoControl::SERVO_CENTER + (this->positions[param] * ServoControl::SERVO_RANGE));
         return sizeof(float);
     } else {
         return 0;
     }
-    this->positions[param] = value;
-    servos[param].writeMicroseconds(ServoControl::SERVO_CENTER + (this->positions[param] * ServoControl::SERVO_RANGE));
-    return sizeof(float);
 }
 
 void ServoControl::device_reset() {