ADD measure time call

multi_tasking/pedal_device.cpp

@@ -27,6 +27,7 @@
 // from disco_h747i/wrappers
 #include <chrono>
 
+#include "bike_system.hpp"
 #include "joystick.hpp"
 #include "mbed_trace.h"
 
@@ -36,19 +37,18 @@
 
 namespace multi_tasking {
 
-PedalDevice::PedalDevice(Mail<uint32_t, 16>* mailBox) {
+PedalDevice::PedalDevice(Mail<pedalMail_t, 16>* mailBox, Timer& timer)
+    : _mailBox(mailBox), _timer(timer) {
     disco::Joystick::getInstance().setLeftCallback(callback(this, &PedalDevice::onLeft));
     disco::Joystick::getInstance().setRightCallback(
         callback(this, &PedalDevice::onRight));
-
-    _mailBox = mailBox;
 }
 
 void PedalDevice::increaseRotationSpeed() {
     uint32_t currentStep = core_util_atomic_load_u32(&_currentStep);
     if (currentStep > 0) {
         core_util_atomic_decr_u32(&_currentStep, 1);
-        sendMail(--currentStep);
+        sendMail(core_util_atomic_load_u32(&_currentStep));
     }
 }
 
@@ -56,7 +56,7 @@ void PedalDevice::decreaseRotationSpeed() {
     uint32_t currentStep = core_util_atomic_load_u32(&_currentStep);
     if (currentStep < bike_computer::kNbrOfSteps) {
         core_util_atomic_incr_u32(&_currentStep, 1);
-        sendMail(++currentStep);
+        sendMail(core_util_atomic_load_u32(&_currentStep));
     }
 }
 
@@ -65,14 +65,17 @@ void PedalDevice::onLeft() { decreaseRotationSpeed(); }
 void PedalDevice::onRight() { increaseRotationSpeed(); }
 
 void PedalDevice::sendMail(uint32_t data) {
-    uint32_t* currentStep = _mailBox->try_alloc();
-    if (currentStep != nullptr) {
-        *currentStep = data;
-        (*_mailBox).put(currentStep);
+    if (_mailBox != nullptr) {
+        pedalMail_t* currentStep = _mailBox->try_alloc();
+        if (currentStep != nullptr) {
+            currentStep->step     = data;
+            currentStep->callTime = _timer.elapsed_time();
+            _mailBox->put(currentStep);
+        }
     }
 }
 
-// mbed_build_and_runstatic methods
+// static methods
 
 std::chrono::milliseconds PedalDevice::getCurrentRotationTime(uint32_t step) {
     return bike_computer::kMinPedalRotationTime +