Differentiate between timed and externally triggered sensors.

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() {

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,

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();
+}

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);

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.

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