diff --git a/firmware/libraries/versavis/src/Camera.cpp b/firmware/libraries/versavis/src/Camera.cpp
index 8024de5b..764d4c3a 100644
--- a/firmware/libraries/versavis/src/Camera.cpp
+++ b/firmware/libraries/versavis/src/Camera.cpp
@@ -20,7 +20,7 @@ Camera::Camera(ros::NodeHandle *nh, const String &topic, const int rate_hz,
                const uint8_t trigger_pin /*= 0 */,
                const uint8_t exposure_pin /*= 0 */,
                const bool exposure_compensation /*= true*/)
-    : Sensor(nh, topic, rate_hz, timer, image_time_msg_, type),
+    : TimedSensor(nh, topic, rate_hz, timer, image_time_msg_, type),
       trigger_pin_(trigger_pin), exposure_pin_(exposure_pin),
       exposure_compensation_(exposure_compensation), is_configured_(true),
       compensating_(false), exposing_(false), image_number_(0),
@@ -81,7 +81,7 @@ void Camera::begin() {
   max_exposure_time_us_ = 0.99 * 1e6 / rate_hz_;
 
   // Setup timer to periodically trigger the camera.
-  Sensor::setupTimer();
+  TimedSensor::setupTimer();
 }
 
 void Camera::setupPublisher() {
diff --git a/firmware/libraries/versavis/src/Camera.h b/firmware/libraries/versavis/src/Camera.h
index f65035fc..eb1301b4 100644
--- a/firmware/libraries/versavis/src/Camera.h
+++ b/firmware/libraries/versavis/src/Camera.h
@@ -15,13 +15,13 @@
 #define Camera_h
 
 #include "Arduino.h"
-#include "Sensor.h"
+#include "TimedSensor.h"
 #include <ros.h>
 #include <std_msgs/Bool.h>
 #include <versavis/TimeNumbered.h>
 
 // timers Class Definition
-class Camera : public Sensor {
+class Camera : public TimedSensor {
 public:
   Camera(ros::NodeHandle *nh, const String &topic, const int rate_hz,
          Timer &timer, const trigger_type &type, const uint8_t trigger_pin = 0,
diff --git a/firmware/libraries/versavis/src/Imu.cpp b/firmware/libraries/versavis/src/Imu.cpp
index 7b95e03e..5a0609fb 100644
--- a/firmware/libraries/versavis/src/Imu.cpp
+++ b/firmware/libraries/versavis/src/Imu.cpp
@@ -1,74 +1,74 @@
-#include "Arduino.h"
-#include "Imu.h"
-#include "helper.h"
-#include "versavis_configuration.h"
-
-Imu::Imu(ros::NodeHandle *nh, const String &topic, const int rate_hz,
-         Timer &timer)
-    : Sensor(nh, topic, rate_hz, timer, imu_msg_), kMaxRecursiveUpdateDepth(5u),
-      kImuSyncTimeoutUs(4000) {}
-
-void Imu::triggerMeasurement() {
-  // Check whether an overflow caused the interrupt.
-  if (!timer_.checkOverflow()) {
-    DEBUG_PRINTLN(
-        (topic_ + " (Imu.cpp): Timer interrupt but not overflown.").c_str());
-    return;
-  }
-
-  DEBUG_PRINTLN((topic_ + " (Imu.cpp): Trigger.").c_str());
-  Sensor::setTimestampNow();
-  Sensor::newMeasurementIsAvailable();
-
-#ifdef ADD_TRIGGERS
-  trigger(ADDITIONAL_TEST_PIN, TRIGGER_PULSE_US,
-          Sensor::trigger_type::NON_INVERTED);
-#endif
-  // Reset the timer.
-  timer_.resetOverflow();
-}
-
-void Imu::publish() {
-  if (Sensor::isNewMeasurementAvailable()) {
-    DEBUG_PRINTLN((topic_ + " (Imu.cpp): Publish."));
-    bool success;
-#ifdef DEBUG
-    success = updateDataIterative();
-#else
-    success = updateDataIterative();
-#endif
-    if (!success) {
-      Sensor::newMeasurementIsNotAvailable();
-      DEBUG_PRINTLN((topic_ + " (Imu.cpp): IMU update failed.").c_str());
-    } else {
-      imu_msg_.time.data = Sensor::getTimestamp();
-      if (sensor_data_ == nullptr) {
-        error((topic_ + " (Imu.cpp): sensor_data == nullptr").c_str(), 10);
-      }
-      imu_msg_.gx = sensor_data_[ImuReading::GX];
-      imu_msg_.gy = sensor_data_[ImuReading::GY];
-      imu_msg_.gz = sensor_data_[ImuReading::GZ];
-      imu_msg_.ax = sensor_data_[ImuReading::AX];
-      imu_msg_.ay = sensor_data_[ImuReading::AY];
-      imu_msg_.az = sensor_data_[ImuReading::AZ];
-#ifndef DEBUG
-      publisher_.publish(&imu_msg_);
-#endif
-      Sensor::newMeasurementIsNotAvailable();
-    }
-  }
-}
-
-void Imu::setupPublisher() {
-  publisher_ = ros::Publisher(topic_.c_str(), &imu_msg_);
-  DEBUG_PRINT((topic_ + " (Imu.cpp): Setup publisher with topic ").c_str());
-  DEBUG_PRINTLN(publisher_.topic_);
-#ifndef DEBUG
-  nh_->advertise(publisher_);
-#endif
-}
-
-void Imu::begin() {
-  DEBUG_PRINTLN((topic_ + " (Imu.cpp): Begin.").c_str());
-  Sensor::setupTimer();
-}
+#include "Arduino.h"
+#include "Imu.h"
+#include "helper.h"
+#include "versavis_configuration.h"
+
+Imu::Imu(ros::NodeHandle *nh, const String &topic, const int rate_hz,
+         Timer &timer)
+    : TimedSensor(nh, topic, rate_hz, timer, imu_msg_), kMaxRecursiveUpdateDepth(5u),
+      kImuSyncTimeoutUs(4000) {}
+
+void Imu::triggerMeasurement() {
+  // Check whether an overflow caused the interrupt.
+  if (!timer_.checkOverflow()) {
+    DEBUG_PRINTLN(
+        (topic_ + " (Imu.cpp): Timer interrupt but not overflown.").c_str());
+    return;
+  }
+
+  DEBUG_PRINTLN((topic_ + " (Imu.cpp): Trigger.").c_str());
+  Sensor::setTimestampNow();
+  Sensor::newMeasurementIsAvailable();
+
+#ifdef ADD_TRIGGERS
+  trigger(ADDITIONAL_TEST_PIN, TRIGGER_PULSE_US,
+          Sensor::trigger_type::NON_INVERTED);
+#endif
+  // Reset the timer.
+  timer_.resetOverflow();
+}
+
+void Imu::publish() {
+  if (Sensor::isNewMeasurementAvailable()) {
+    DEBUG_PRINTLN((topic_ + " (Imu.cpp): Publish."));
+    bool success;
+#ifdef DEBUG
+    success = updateDataIterative();
+#else
+    success = updateDataIterative();
+#endif
+    if (!success) {
+      Sensor::newMeasurementIsNotAvailable();
+      DEBUG_PRINTLN((topic_ + " (Imu.cpp): IMU update failed.").c_str());
+    } else {
+      imu_msg_.time.data = Sensor::getTimestamp();
+      if (sensor_data_ == nullptr) {
+        error((topic_ + " (Imu.cpp): sensor_data == nullptr").c_str(), 10);
+      }
+      imu_msg_.gx = sensor_data_[ImuReading::GX];
+      imu_msg_.gy = sensor_data_[ImuReading::GY];
+      imu_msg_.gz = sensor_data_[ImuReading::GZ];
+      imu_msg_.ax = sensor_data_[ImuReading::AX];
+      imu_msg_.ay = sensor_data_[ImuReading::AY];
+      imu_msg_.az = sensor_data_[ImuReading::AZ];
+#ifndef DEBUG
+      publisher_.publish(&imu_msg_);
+#endif
+      Sensor::newMeasurementIsNotAvailable();
+    }
+  }
+}
+
+void Imu::setupPublisher() {
+  publisher_ = ros::Publisher(topic_.c_str(), &imu_msg_);
+  DEBUG_PRINT((topic_ + " (Imu.cpp): Setup publisher with topic ").c_str());
+  DEBUG_PRINTLN(publisher_.topic_);
+#ifndef DEBUG
+  nh_->advertise(publisher_);
+#endif
+}
+
+void Imu::begin() {
+  DEBUG_PRINTLN((topic_ + " (Imu.cpp): Begin.").c_str());
+  TimedSensor::setupTimer();
+}
diff --git a/firmware/libraries/versavis/src/Imu.h b/firmware/libraries/versavis/src/Imu.h
index b4fc7224..785782d3 100644
--- a/firmware/libraries/versavis/src/Imu.h
+++ b/firmware/libraries/versavis/src/Imu.h
@@ -14,7 +14,7 @@
 #define Imu_h
 
 #include "Arduino.h"
-#include "Sensor.h"
+#include "TimedSensor.h"
 #include <ros.h>
 #include <versavis/ImuMicro.h>
 
@@ -30,7 +30,7 @@ enum ImuReading {
   // Check burst read function of the specific IMU.
 };
 
-class Imu : public Sensor {
+class Imu : public TimedSensor {
 public:
   Imu(ros::NodeHandle *nh, const String &topic, const int rate_hz,
       Timer &timer);
diff --git a/firmware/libraries/versavis/src/Sensor.cpp b/firmware/libraries/versavis/src/Sensor.cpp
index 40d82aa6..39d5e8ef 100644
--- a/firmware/libraries/versavis/src/Sensor.cpp
+++ b/firmware/libraries/versavis/src/Sensor.cpp
@@ -4,40 +4,26 @@
 #include "versavis_configuration.h"
 
 // Image time message version.
-Sensor::Sensor(ros::NodeHandle *nh, const String &topic, const int rate_hz,
-               Timer &timer, versavis::TimeNumbered &img_time_msg,
+Sensor::Sensor(ros::NodeHandle *nh, const String &topic,
+               versavis::TimeNumbered &img_time_msg,
                const trigger_type type /* = trigger_type::NON_INVERTED */)
     : nh_(nh), topic_(topic),
-      publisher_((topic + "img_time").c_str(), &img_time_msg),
-      new_measurement_available_(false), rate_hz_(rate_hz), timer_(timer),
-      type_(type), max_compare_(pow(2, 16)) {
+      publisher_((topic + "img_time").c_str(), &img_time_msg), type_(type) {
   if (nh == nullptr) {
     error((topic_ + " (Sensor.cpp): The node handle is not available.").c_str(),
           49);
   }
-  if (rate_hz <= 0) {
-    error((topic_ + " (Sensor.cpp): The rate of a sensor needs to be positive.")
-              .c_str(),
-          50);
-  }
 }
 
 // IMU message version.
-Sensor::Sensor(ros::NodeHandle *nh, const String &topic, const int rate_hz,
-               Timer &timer, versavis::ImuMicro &imu_msg,
+Sensor::Sensor(ros::NodeHandle *nh, const String &topic,
+               versavis::ImuMicro &imu_msg,
                const trigger_type type /* = trigger_type::NON_INVERTED */)
-    : nh_(nh), topic_(topic), publisher_(topic.c_str(), &imu_msg),
-      new_measurement_available_(false), rate_hz_(rate_hz), timer_(timer),
-      type_(type), max_compare_(pow(2, 16)) {
+    : nh_(nh), topic_(topic), publisher_(topic.c_str(), &imu_msg), type_(type) {
   if (nh == nullptr) {
     error((topic_ + " (Sensor.cpp): The node handle is not available.").c_str(),
           49);
   }
-  if (rate_hz <= 0) {
-    error((topic_ + " (Sensor.cpp): The rate of a sensor needs to be positive.")
-              .c_str(),
-          50);
-  }
   if (topic.length() == 0) {
     error((topic_ + " (Sensor.cpp): IMU topic is empty.").c_str(), 51);
   }
@@ -57,51 +43,6 @@ void Sensor::newMeasurementIsNotAvailable() {
   new_measurement_available_ = false;
 }
 
-uint16_t Sensor::calculatePrescaler(const uint16_t rate_hz) const {
-  // All timers on Arduino Zero are assumed to be set to 16 bit.
-  const uint32_t kMaxCount = 65536;
-
-  // Amount of clock ticks required to count to the specified period time.
-  const uint32_t kRequiredTicks = CPU_FREQ_HZ / static_cast<double>(rate_hz);
-  DEBUG_PRINT((topic_ + " (Sensor.cpp): required ticks "));
-  DEBUG_PRINTDECLN(kRequiredTicks);
-  // Available prescalers on Arduino Zero are restricted to those values.
-  const uint16_t kAvailablePrescalers[8] = {1, 2, 4, 8, 16, 64, 256, 1024};
-
-  for (uint8_t i = 0; i < 8; ++i) {
-    if (kMaxCount > kRequiredTicks / kAvailablePrescalers[i] &&
-        kRequiredTicks % kAvailablePrescalers[i] == 0) {
-      return kAvailablePrescalers[i];
-    }
-  }
-  // If this part is reached, no available prescaler fits with the goal
-  // framerate on this MCU.
-  error((topic_ + " (Sensor.cpp): No prescaler found.").c_str(), 50);
-  return 0;
-}
-
-void Sensor::setupTimer() {
-  prescaler_ = calculatePrescaler(rate_hz_);
-  if (prescaler_ == 0) {
-    error((topic_ + " (Sensor.cpp): prescaler_ is zero.").c_str(), 99);
-  }
-  cpu_freq_prescaler_ = CPU_FREQ_HZ / prescaler_;
-  DEBUG_PRINT((topic_ + " (Sensor.cpp): Setup timer with prescaler ").c_str());
-  DEBUG_PRINTDECLN(prescaler_);
-  // The compare value defines at which tick the timer is going to overflow
-  // and triggering the corresponding interrupt.
-  compare_ = 1.0 / rate_hz_ * cpu_freq_prescaler_ - 1;
-  if (compare_ > max_compare_) {
-    // The compare value can never be reached by a 16bit timer. Choose a
-    // higher prescaler or another timer with 32bit.
-    error((topic_ + " (Sensor.cpp): compare_ > pow(2,16) failed.").c_str(),
-          100);
-  }
-
-  // Actually setting up the timer registers.
-  timer_.initialize(prescaler_, compare_, topic_);
-}
-
 void Sensor::trigger(const int pin, const int pulse_time_us,
                      const trigger_type &type) {
   // Perform the trigger pulse.
diff --git a/firmware/libraries/versavis/src/Sensor.h b/firmware/libraries/versavis/src/Sensor.h
index a92ae330..877bd9dd 100644
--- a/firmware/libraries/versavis/src/Sensor.h
+++ b/firmware/libraries/versavis/src/Sensor.h
@@ -23,11 +23,10 @@ enum trigger_type { INVERTED, NON_INVERTED };
 
 class Sensor {
 public:
-  Sensor(ros::NodeHandle *nh, const String &topic, const int rate_hz,
-         Timer &timer, versavis::TimeNumbered &img_time_msg,
+  Sensor(ros::NodeHandle *nh, const String &topic,
+         versavis::TimeNumbered &img_time_msg,
          const trigger_type type = trigger_type::NON_INVERTED);
-  Sensor(ros::NodeHandle *nh, const String &topic, const int rate_hz,
-         Timer &timer, versavis::ImuMicro &imu_msg,
+  Sensor(ros::NodeHandle *nh, const String &topic, versavis::ImuMicro &imu_msg,
          const trigger_type type = trigger_type::NON_INVERTED);
   inline virtual void setup(){/* do nothing */};
   inline virtual void begin(){/* do nothing */};
@@ -41,41 +40,22 @@ class Sensor {
   void newMeasurementIsAvailable();
   void newMeasurementIsNotAvailable();
 
-  void setupTimer();
-
   void trigger(const int pin, const int pulse_time_us,
                const trigger_type &type);
 
-  // ------------ROS members-----------
 protected:
+  // Trigger type. Non-inverted: The logic level is at 0 and goes to 1 for
+  // triggering. Inverted: The logic level is at 1 and goes to 0 for triggering.
+  const trigger_type type_;
+  
+  // ------------ROS members-----------
   ros::NodeHandle *nh_;
   String topic_;
   ros::Publisher publisher_;
 
 private:
   ros::Time timestamp_;
-  bool new_measurement_available_;
-
-  //------------Timer members-----------
-protected:
-  // Frequency of the timer in Hz.
-  uint16_t rate_hz_;
-  // The timer object (can be TCC or TcCount16).
-  Timer timer_;
-  // Trigger type. Non-inverted: The logic level is at 0 and goes to 1 for
-  // triggering. Inverted: The logic level is at 1 and goes to 0 for triggering.
-  const trigger_type type_;
-
-  // Compare register to use for timer overlow / interrupt.
-  unsigned long compare_;
-  const unsigned long max_compare_;
-  // Prescaler of the timer, which is set automatically.
-  uint16_t prescaler_;
-  double cpu_freq_prescaler_;
-
-private:
-  // Function to determine prescaler based on timer size and frequency.
-  uint16_t calculatePrescaler(const uint16_t rate_hz) const;
+  bool new_measurement_available_ = false;
 };
 
 #endif
diff --git a/firmware/libraries/versavis/src/TimedSensor.cpp b/firmware/libraries/versavis/src/TimedSensor.cpp
new file mode 100644
index 00000000..b7ec982b
--- /dev/null
+++ b/firmware/libraries/versavis/src/TimedSensor.cpp
@@ -0,0 +1,78 @@
+#include "Arduino.h"
+#include "TimedSensor.h"
+#include "helper.h"
+#include "versavis_configuration.h"
+
+// Image time message version.
+TimedSensor::TimedSensor(
+    ros::NodeHandle *nh, const String &topic, const int rate_hz, Timer &timer,
+    versavis::TimeNumbered &img_time_msg,
+    const trigger_type type /* = trigger_type::NON_INVERTED */)
+    : Sensor(nh, topic, img_time_msg, type), rate_hz_(rate_hz), timer_(timer) {
+  if (rate_hz <= 0) {
+    error((topic_ +
+           " (TimedSensor.cpp): The rate of a sensor needs to be positive.")
+              .c_str(),
+          50);
+  }
+}
+
+// IMU message version.
+TimedSensor::TimedSensor(
+    ros::NodeHandle *nh, const String &topic, const int rate_hz, Timer &timer,
+    versavis::ImuMicro &imu_msg,
+    const trigger_type type /* = trigger_type::NON_INVERTED */)
+    : Sensor(nh, topic, imu_msg, type), rate_hz_(rate_hz), timer_(timer) {
+  if (rate_hz <= 0) {
+    error((topic_ +
+           " (TimedSensor.cpp): The rate of a sensor needs to be positive.")
+              .c_str(),
+          50);
+  }
+}
+
+uint16_t TimedSensor::calculatePrescaler(const uint16_t rate_hz) const {
+  // All timers on Arduino Zero are assumed to be set to 16 bit.
+  const uint32_t kMaxCount = 65536;
+
+  // Amount of clock ticks required to count to the specified period time.
+  const uint32_t kRequiredTicks = CPU_FREQ_HZ / static_cast<double>(rate_hz);
+  DEBUG_PRINT((topic_ + " (TimedSensor.cpp): required ticks "));
+  DEBUG_PRINTDECLN(kRequiredTicks);
+  // Available prescalers on Arduino Zero are restricted to those values.
+  const uint16_t kAvailablePrescalers[8] = {1, 2, 4, 8, 16, 64, 256, 1024};
+
+  for (uint8_t i = 0; i < 8; ++i) {
+    if (kMaxCount > kRequiredTicks / kAvailablePrescalers[i] &&
+        kRequiredTicks % kAvailablePrescalers[i] == 0) {
+      return kAvailablePrescalers[i];
+    }
+  }
+  // If this part is reached, no available prescaler fits with the goal
+  // framerate on this MCU.
+  error((topic_ + " (TimedSensor.cpp): No prescaler found.").c_str(), 50);
+  return 0;
+}
+
+void TimedSensor::setupTimer() {
+  prescaler_ = calculatePrescaler(rate_hz_);
+  if (prescaler_ == 0) {
+    error((topic_ + " (TimedSensor.cpp): prescaler_ is zero.").c_str(), 99);
+  }
+  cpu_freq_prescaler_ = CPU_FREQ_HZ / prescaler_;
+  DEBUG_PRINT(
+      (topic_ + " (TimedSensor.cpp): Setup timer with prescaler ").c_str());
+  DEBUG_PRINTDECLN(prescaler_);
+  // The compare value defines at which tick the timer is going to overflow
+  // and triggering the corresponding interrupt.
+  compare_ = 1.0 / rate_hz_ * cpu_freq_prescaler_ - 1;
+  if (compare_ > max_compare_) {
+    // The compare value can never be reached by a 16bit timer. Choose a
+    // higher prescaler or another timer with 32bit.
+    error((topic_ + " (TimedSensor.cpp): compare_ > pow(2,16) failed.").c_str(),
+          100);
+  }
+
+  // Actually setting up the timer registers.
+  timer_.initialize(prescaler_, compare_, topic_);
+}
diff --git a/firmware/libraries/versavis/src/TimedSensor.h b/firmware/libraries/versavis/src/TimedSensor.h
new file mode 100644
index 00000000..90058ded
--- /dev/null
+++ b/firmware/libraries/versavis/src/TimedSensor.h
@@ -0,0 +1,53 @@
+////////////////////////////////////////////////////////////////////////////////
+//  January 2020
+//  Author: Florian Tschopp <ftschopp@ethz.ch>
+////////////////////////////////////////////////////////////////////////////////
+//  TimedSensor.h
+////////////////////////////////////////////////////////////////////////////////
+//
+//  Basic implementation for generic sensors in the versavis framework that is
+//  controlled by a timer.
+//
+////////////////////////////////////////////////////////////////////////////////
+
+#ifndef TimedSensor_h
+#define TimedSensor_h
+
+#include "Arduino.h"
+#include "Sensor.h"
+#include "Timer.h"
+#include <ros.h>
+#include <versavis/ImuMicro.h>
+#include <versavis/TimeNumbered.h>
+
+class TimedSensor : public Sensor {
+public:
+  TimedSensor(ros::NodeHandle *nh, const String &topic, const int rate_hz,
+              Timer &timer, versavis::TimeNumbered &img_time_msg,
+              const trigger_type type = trigger_type::NON_INVERTED);
+  TimedSensor(ros::NodeHandle *nh, const String &topic, const int rate_hz,
+              Timer &timer, versavis::ImuMicro &imu_msg,
+              const trigger_type type = trigger_type::NON_INVERTED);
+
+  void setupTimer();
+
+  //------------Timer members-----------
+protected:
+  // Frequency of the timer in Hz.
+  uint16_t rate_hz_;
+  // The timer object (can be TCC or TcCount16).
+  Timer timer_;
+
+  // Compare register to use for timer overlow / interrupt.
+  unsigned long compare_;
+  const unsigned long max_compare_ = pow(2, 16);
+  // Prescaler of the timer, which is set automatically.
+  uint16_t prescaler_;
+  double cpu_freq_prescaler_;
+
+private:
+  // Function to determine prescaler based on timer size and frequency.
+  uint16_t calculatePrescaler(const uint16_t rate_hz) const;
+};
+
+#endif