365 lines
12 KiB
C++
365 lines
12 KiB
C++
// Copyright 2022 Haute école d'ingénierie et d'architecture de Fribourg
|
|
//
|
|
// Licensed under the Apache License, Version 2.0 (the "License");
|
|
// you may not use this file except in compliance with the License.
|
|
// You may obtain a copy of the License at
|
|
//
|
|
// http://www.apache.org/licenses/LICENSE-2.0
|
|
//
|
|
// Unless required by applicable law or agreed to in writing, software
|
|
// distributed under the License is distributed on an "AS IS" BASIS,
|
|
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
// See the License for the specific language governing permissions and
|
|
// limitations under the License.
|
|
|
|
/****************************************************************************
|
|
* @file bike_system.cpp
|
|
* @author Serge Ayer <serge.ayer@hefr.ch>
|
|
* @author Rémi Heredero <remi@heredero.ch>
|
|
* @author Yann Sierro <yannsierro.pro@gmail.com>
|
|
*
|
|
* @brief Bike System implementation (static scheduling)
|
|
*
|
|
* @date 2023-11-15
|
|
* @version 1.1.0
|
|
***************************************************************************/
|
|
|
|
#include "bike_system.hpp"
|
|
|
|
#include <chrono>
|
|
|
|
#include "Callback.h"
|
|
#include "cmsis_os2.h"
|
|
#include "constants.hpp"
|
|
#include "mbed_trace.h"
|
|
#include "pedal_device.hpp"
|
|
#if MBED_CONF_MBED_TRACE_ENABLE
|
|
#define TRACE_GROUP "BikeSystem"
|
|
#endif // MBED_CONF_MBED_TRACE_ENABLE
|
|
|
|
namespace multi_tasking {
|
|
|
|
static constexpr std::chrono::milliseconds kGearTaskPeriod = 800ms;
|
|
static constexpr std::chrono::milliseconds kGearTaskDelay = 0ms;
|
|
static constexpr std::chrono::milliseconds kGearTaskComputationTime = 100ms;
|
|
static constexpr std::chrono::milliseconds kSpeedDistanceTaskPeriod = 400ms;
|
|
static constexpr std::chrono::milliseconds kSpeedDistanceTaskDelay = 0ms; // 0 or 100ms
|
|
static constexpr std::chrono::milliseconds kSpeedDistanceTaskComputationTime = 200ms;
|
|
static constexpr std::chrono::milliseconds kDisplayTask1Period = 1600ms;
|
|
static constexpr std::chrono::milliseconds kDisplayTask1Delay = 300ms;
|
|
static constexpr std::chrono::milliseconds kDisplayTask1ComputationTime = 200ms;
|
|
static constexpr std::chrono::milliseconds kResetTaskPeriod = 800ms;
|
|
static constexpr std::chrono::milliseconds kResetTaskDelay = 700ms;
|
|
static constexpr std::chrono::milliseconds kResetTaskComputationTime = 100ms;
|
|
static constexpr std::chrono::milliseconds kTemperatureTaskPeriod = 1600ms;
|
|
static constexpr std::chrono::milliseconds kTemperatureTaskDelay = 1100ms;
|
|
static constexpr std::chrono::milliseconds kTemperatureTaskComputationTime = 100ms;
|
|
static constexpr std::chrono::milliseconds kDisplayTask2Period = 1600ms;
|
|
static constexpr std::chrono::milliseconds kDisplayTask2Delay = 1200ms;
|
|
static constexpr std::chrono::milliseconds kDisplayTask2ComputationTime = 100ms;
|
|
static constexpr std::chrono::milliseconds kCPUTaskPeriod = 1600ms;
|
|
static constexpr std::chrono::milliseconds kCPUTaskDelay = 0ms;
|
|
static constexpr std::chrono::milliseconds kCPUTaskComputationTime = 0ms;
|
|
|
|
BikeSystem::BikeSystem()
|
|
: _gearDevice(callback(this, &BikeSystem::onGearUp),
|
|
callback(this, &BikeSystem::onGearDown)),
|
|
_pedalDevice(&_mailPedalDevice),
|
|
_resetDevice(callback(this, &BikeSystem::onReset)),
|
|
_speedometer(_timer),
|
|
_cpuLogger(_timer),
|
|
_isrEventThread(osPriorityAboveNormal, OS_STACK_SIZE, nullptr, "ISR_Event"),
|
|
_speedDistanceThread(
|
|
osPriorityNormal, OS_STACK_SIZE, nullptr, "Speed_distance_Task") {}
|
|
#if defined(MBED_TEST_MODE)
|
|
const advembsof::TaskLogger& BikeSystem::getTaskLogger() { return _taskLogger; }
|
|
#endif // defined(MBED_TEST_MODE)
|
|
|
|
void BikeSystem::init() {
|
|
// start the timer
|
|
_timer.start();
|
|
|
|
// initialize the lcd display
|
|
disco::ReturnCode rc = _displayDevice.init();
|
|
if (rc != disco::ReturnCode::Ok) {
|
|
tr_error("Ffalseailed to initialized the lcd display: %ld", static_cast<int>(rc));
|
|
}
|
|
|
|
// initialize the sensor device
|
|
bool present = _sensorDevice.init();
|
|
if (!present) {
|
|
tr_error("Sensor not present or initialization failed");
|
|
}
|
|
|
|
// enable/disable task logging
|
|
_taskLogger.enable(false);
|
|
}
|
|
|
|
void BikeSystem::start() {
|
|
tr_info("Starting Super-Loop with event handling");
|
|
|
|
init();
|
|
|
|
Event<void()> speedDistanceEvent(&_eventQueue,
|
|
callback(this, &BikeSystem::speedDistanceTask));
|
|
speedDistanceEvent.delay(kSpeedDistanceTaskDelay);
|
|
speedDistanceEvent.period(kSpeedDistanceTaskPeriod);
|
|
speedDistanceEvent.post();
|
|
|
|
Event<void()> display1Event(&_eventQueue, callback(this, &BikeSystem::displayTask1));
|
|
display1Event.delay(kDisplayTask1Delay);
|
|
display1Event.period(kDisplayTask1Period);
|
|
display1Event.post();
|
|
|
|
Event<void()> temperatureEvent(&_eventQueue,
|
|
callback(this, &BikeSystem::temperatureTask));
|
|
temperatureEvent.delay(kTemperatureTaskDelay);
|
|
temperatureEvent.period(kTemperatureTaskPeriod);
|
|
temperatureEvent.post();
|
|
|
|
Event<void()> display2Event(&_eventQueue, callback(this, &BikeSystem::displayTask2));
|
|
display2Event.delay(kDisplayTask2Delay);
|
|
display2Event.period(kDisplayTask2Period);
|
|
display2Event.post();
|
|
|
|
osStatus status =
|
|
_isrEventThread.start(callback(this, &BikeSystem::dispatch_isr_events));
|
|
if (status != osOK) {
|
|
tr_error("Thread %s started with status %d", _isrEventThread.get_name(), status);
|
|
}
|
|
|
|
status =
|
|
_speedDistanceThread.start(callback(this, &BikeSystem::loop_speed_distance_task));
|
|
if (status != osOK) {
|
|
tr_error("Thread %s started with status %d", _isrEventThread.get_name(), status);
|
|
}
|
|
|
|
// dispathc the main queue in the main thread
|
|
dispatch_events();
|
|
}
|
|
|
|
/* Callback from isr */
|
|
|
|
void BikeSystem::onReset() {
|
|
_resetTime = _timer.elapsed_time();
|
|
Event<void()> resetEvent(&_isrEventQueue, callback(this, &BikeSystem::resetTask));
|
|
resetEvent.post();
|
|
}
|
|
|
|
void BikeSystem::onGearUp() {
|
|
_onGearUpTime = _timer.elapsed_time();
|
|
Event<void()> gearUpEvent(&_isrEventQueue, callback(this, &BikeSystem::gearUpTask));
|
|
gearUpEvent.post();
|
|
}
|
|
|
|
void BikeSystem::onGearDown() {
|
|
_onGearDownTime = _timer.elapsed_time();
|
|
Event<void()> gearDownEvent(&_isrEventQueue,
|
|
callback(this, &BikeSystem::gearDownTask));
|
|
gearDownEvent.post();
|
|
}
|
|
|
|
// ISR thread functions
|
|
|
|
void BikeSystem::resetTask() {
|
|
auto taskStartTime = _timer.elapsed_time();
|
|
|
|
std::chrono::microseconds responseTime = _timer.elapsed_time() - _resetTime;
|
|
tr_info("Reset task: response time is %" PRIu64 " usecs", responseTime.count());
|
|
_speedometer.reset();
|
|
|
|
_taskLogger.logPeriodAndExecutionTime(
|
|
_timer, advembsof::TaskLogger::kResetTaskIndex, taskStartTime);
|
|
}
|
|
|
|
// Speed distance thread functions
|
|
|
|
void BikeSystem::speedDistanceTask() {
|
|
auto taskStartTime = _timer.elapsed_time();
|
|
|
|
uint32_t* currentStep = _mailPedalDevice.try_get();
|
|
|
|
if (currentStep != nullptr) {
|
|
const auto pedalRotationTime = PedalDevice::getCurrentRotationTime(*currentStep);
|
|
|
|
osStatus status = _mailPedalDevice.free(currentStep);
|
|
if (status != osOK) {
|
|
tr_error("free current step in the speed distance tasks doesn't work !");
|
|
}
|
|
|
|
// ENTER CRITICAL SECTION
|
|
_mutexSpeedometer.lock();
|
|
_speedometer.setCurrentRotationTime(pedalRotationTime);
|
|
_mutexSpeedometer.unlock();
|
|
// END CRITICAL SECTION
|
|
}
|
|
|
|
// ENTER CRITICAL SECTION
|
|
_mutexSpeedometer.lock();
|
|
_speedometer.setGearSize(getCurrentGearSize());
|
|
_mutexSpeed.lock();
|
|
_currentSpeed = _speedometer.getCurrentSpeed();
|
|
_mutexSpeed.unlock();
|
|
_mutexDistance.lock();
|
|
_traveledDistance = _speedometer.getDistance();
|
|
_mutexDistance.unlock();
|
|
_mutexSpeedometer.unlock();
|
|
// END CRITICAL SECTION
|
|
|
|
_taskLogger.logPeriodAndExecutionTime(
|
|
_timer, advembsof::TaskLogger::kSpeedTaskIndex, taskStartTime);
|
|
}
|
|
|
|
/* Main thread functions */
|
|
|
|
void BikeSystem::gearUpTask() {
|
|
auto taskStartTime = _timer.elapsed_time();
|
|
std::chrono::microseconds responseTime = _timer.elapsed_time() - _onGearUpTime;
|
|
tr_info("Gear up task: response time is %" PRIu64 " usecs", responseTime.count());
|
|
|
|
// ENTER CRITICAL SECTION
|
|
_mutexGear.lock();
|
|
if (_currentGear < bike_computer::kMaxGear) {
|
|
_currentGear++;
|
|
_mutexGearSize.lock();
|
|
_currentGearSize = bike_computer::kMaxGearSize - _currentGear;
|
|
_mutexGearSize.unlock();
|
|
}
|
|
_mutexGear.unlock();
|
|
// END CRITICAL SECTION
|
|
|
|
_taskLogger.logPeriodAndExecutionTime(
|
|
_timer, advembsof::TaskLogger::kGearTaskIndex, taskStartTime);
|
|
}
|
|
void BikeSystem::gearDownTask() {
|
|
auto taskStartTime = _timer.elapsed_time();
|
|
std::chrono::microseconds responseTime = _timer.elapsed_time() - _onGearDownTime;
|
|
tr_info("Gear down task: response time is %" PRIu64 " usecs", responseTime.count());
|
|
|
|
// ENTER CRITICAL SECTION
|
|
_mutexGear.lock();
|
|
if (_currentGear > bike_computer::kMinGear) {
|
|
_currentGear--;
|
|
_mutexGearSize.lock();
|
|
_currentGearSize = bike_computer::kMaxGearSize - _currentGear;
|
|
_mutexGearSize.unlock();
|
|
}
|
|
_mutexGear.unlock();
|
|
// END CRITICAL SECTION
|
|
|
|
_taskLogger.logPeriodAndExecutionTime(
|
|
_timer, advembsof::TaskLogger::kGearTaskIndex, taskStartTime);
|
|
}
|
|
|
|
void BikeSystem::temperatureTask() {
|
|
auto taskStartTime = _timer.elapsed_time();
|
|
|
|
// no need to protect access to data members (single threaded)
|
|
_currentTemperature = _sensorDevice.readTemperature();
|
|
|
|
ThisThread::sleep_for(std::chrono::duration_cast<std::chrono::milliseconds>(
|
|
kTemperatureTaskComputationTime - (_timer.elapsed_time() - taskStartTime)));
|
|
|
|
_taskLogger.logPeriodAndExecutionTime(
|
|
_timer, advembsof::TaskLogger::kTemperatureTaskIndex, taskStartTime);
|
|
}
|
|
|
|
void BikeSystem::displayTask1() {
|
|
auto taskStartTime = _timer.elapsed_time();
|
|
|
|
// ENTER CRITICAL SECTION
|
|
_displayDevice.displayGear(getCurrentGear());
|
|
_displayDevice.displaySpeed(getCurrentSpeed());
|
|
_displayDevice.displayDistance(getCurrentDistance());
|
|
// END CRITICAL SECTION
|
|
|
|
ThisThread::sleep_for(std::chrono::duration_cast<std::chrono::milliseconds>(
|
|
kDisplayTask1ComputationTime - (_timer.elapsed_time() - taskStartTime)));
|
|
|
|
_taskLogger.logPeriodAndExecutionTime(
|
|
_timer, advembsof::TaskLogger::kDisplayTask1Index, taskStartTime);
|
|
}
|
|
|
|
void BikeSystem::displayTask2() {
|
|
auto taskStartTime = _timer.elapsed_time();
|
|
|
|
_displayDevice.displayTemperature(_currentTemperature);
|
|
|
|
ThisThread::sleep_for(std::chrono::duration_cast<std::chrono::milliseconds>(
|
|
kDisplayTask2ComputationTime - (_timer.elapsed_time() - taskStartTime)));
|
|
|
|
_taskLogger.logPeriodAndExecutionTime(
|
|
_timer, advembsof::TaskLogger::kDisplayTask2Index, taskStartTime);
|
|
}
|
|
|
|
void BikeSystem::cpuTask() { _cpuLogger.printStats(); }
|
|
|
|
void BikeSystem::dispatch_isr_events() {
|
|
tr_info("Start dispatching isr events");
|
|
_isrEventQueue.dispatch_forever();
|
|
tr_info("Stop dispatching isr events");
|
|
}
|
|
|
|
void BikeSystem::dispatch_events() {
|
|
tr_info("Start dispatching main events");
|
|
_eventQueue.dispatch_forever();
|
|
tr_info("Stop dispatching main events");
|
|
}
|
|
|
|
void BikeSystem::loop_speed_distance_task() {
|
|
tr_info("Start loop speed-distance calculation");
|
|
while (true) {
|
|
speedDistanceTask();
|
|
}
|
|
}
|
|
|
|
uint8_t BikeSystem::getCurrentGear() {
|
|
uint8_t currentGear;
|
|
|
|
// ENTER CRITICAL SECTION
|
|
_mutexGear.lock();
|
|
currentGear = _currentGear;
|
|
_mutexGear.unlock();
|
|
// END CRITICAL SECTION
|
|
|
|
return currentGear;
|
|
}
|
|
|
|
uint8_t BikeSystem::getCurrentGearSize() {
|
|
uint8_t currentGearSize;
|
|
|
|
// ENTER CRITICAL SECTION
|
|
_mutexGearSize.lock();
|
|
currentGearSize = _currentGearSize;
|
|
_mutexGearSize.unlock();
|
|
// END CRITICAL SECTION
|
|
|
|
return currentGearSize;
|
|
}
|
|
|
|
uint32_t BikeSystem::getCurrentSpeed() {
|
|
uint32_t currentSpeed;
|
|
|
|
// ENTER CRITICAL SECTION
|
|
_mutexSpeed.lock();
|
|
currentSpeed = _currentSpeed;
|
|
_mutexSpeed.unlock();
|
|
// END CRITICAL SECTION
|
|
|
|
return currentSpeed;
|
|
}
|
|
|
|
uint32_t BikeSystem::getCurrentDistance() {
|
|
uint32_t currentDistance;
|
|
|
|
// ENTER CRITICAL SECTION
|
|
_mutexDistance.lock();
|
|
currentDistance = _traveledDistance;
|
|
_mutexDistance.unlock();
|
|
// END CRITICAL SECTION
|
|
|
|
return currentDistance;
|
|
}
|
|
|
|
} // namespace multi_tasking
|