Obbart/AstroRotaxMonitor
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
15ca82b6df1912712619c9eaed16fc07ddb37a0f
AstroRotaxMonitor/RotaxMonitor/src/tasks.cpp

579 lines
22 KiB
C++
Raw Blame History

#include "tasks.h"
#include <esp_timer.h>
#include <datasave.h>
#include <mutex>
//// GLOBAL STATIC FUNCTIONS
// Timeout callback for microsecond precision
void IRAM_ATTR spark_timeout_callback(void *arg)
{
TaskHandle_t handle = (TaskHandle_t)arg;
xTaskNotify(handle, SPARK_FLAG_TIMEOUT, eSetValueWithOverwrite);
}
// Manages queue receive, save data and callback to external tasks for communication
void rtIgnitionTask::rtIgnitionTask_manager(void *pvParameters)
{
rtIgnitionTask *cls = (rtIgnitionTask *)pvParameters;
auto last_loop = millis();
uint32_t count(0);
while (cls->m_running)
{
cls->run();
vTaskDelay(pdMS_TO_TICKS(1));
}
}
// Static task function
void rtIgnitionTask::rtIgnitionTask_realtime(void *pvParameters)
{
// Invalid real time rt_task_ptr parameters, exit immediate
if (!pvParameters)
{
LOG_ERROR("Null rt_task_ptr parameters");
vTaskDelete(NULL);
}
// Task Parameters and Devices
rtTaskParams *params = (rtTaskParams *)pvParameters;
const rtTaskInterruptParams rt_int = params->rt_int; // copy to avoid external override
const rtTaskIOParams rt_rst = params->rt_io; // copy to avoid external override
QueueHandle_t rt_queue = params->rt_queue;
Devices *dev = params->dev;
ExternalIO *io = dev->m_ext_io;
ADS1256 *adc = params->name == "rtIgnTask_A" ? dev->m_adc_a : dev->m_adc_b;
std::mutex &spi_mutex = params->name == "rtIgnTask_A" ? dev->m_spi_a_mutex : dev->m_spi_b_mutex;
TaskStatus_t rt_task_info;
vTaskGetInfo(NULL, &rt_task_info, pdFALSE, eInvalid);
LOG_INFO("rtTask Params OK [", params->name.c_str(), "]");
ignitionBoxStatus ign_box_sts;
// Variables for ISR, static to be fixed in memory locations
isrParams isr_params_t12p{
.flag = TRIG_FLAG_12P,
.ign_stat = &ign_box_sts,
.rt_handle_ptr = rt_task_info.xHandle};
isrParams isr_params_t12n{
.flag = TRIG_FLAG_12N,
.ign_stat = &ign_box_sts,
.rt_handle_ptr = rt_task_info.xHandle};
isrParams isr_params_t34p{
.flag = TRIG_FLAG_34P,
.ign_stat = &ign_box_sts,
.rt_handle_ptr = rt_task_info.xHandle};
isrParams isr_params_t34n{
.flag = TRIG_FLAG_34N,
.ign_stat = &ign_box_sts,
.rt_handle_ptr = rt_task_info.xHandle};
isrParams isr_params_sp12{
.flag = SPARK_FLAG_12,
.ign_stat = &ign_box_sts,
.rt_handle_ptr = rt_task_info.xHandle};
isrParams isr_params_sp34{
.flag = SPARK_FLAG_34,
.ign_stat = &ign_box_sts,
.rt_handle_ptr = rt_task_info.xHandle};
// Create esp_timer for microsecond precision timeout
esp_timer_handle_t timeout_timer;
esp_timer_create_args_t timer_args = {
.callback = spark_timeout_callback,
.arg = (void *)rt_task_info.xHandle,
.dispatch_method = ESP_TIMER_TASK,
.name = "spark_timeout"};
if (esp_timer_create(&timer_args, &timeout_timer) != ESP_OK)
{
LOG_INFO("rtTask [", params->name.c_str(), "] Fail to allocate timeoutTimer");
vTaskDelete(NULL);
}
// Attach Pin Interrupts
attachInterruptArg(digitalPinToInterrupt(rt_int.trig_pin_12p), rt_int.isr_ptr, (void *)&isr_params_t12p, RISING);
attachInterruptArg(digitalPinToInterrupt(rt_int.trig_pin_12n), rt_int.isr_ptr, (void *)&isr_params_t12n, RISING);
attachInterruptArg(digitalPinToInterrupt(rt_int.trig_pin_34p), rt_int.isr_ptr, (void *)&isr_params_t34p, RISING);
attachInterruptArg(digitalPinToInterrupt(rt_int.trig_pin_34n), rt_int.isr_ptr, (void *)&isr_params_t34n, RISING);
attachInterruptArg(digitalPinToInterrupt(rt_int.spark_pin_12), rt_int.isr_ptr, (void *)&isr_params_sp12, RISING);
attachInterruptArg(digitalPinToInterrupt(rt_int.spark_pin_34), rt_int.isr_ptr, (void *)&isr_params_sp34, RISING);
LOG_INFO("rtTask ISR Attach OK [", params->name.c_str(), "]");
// Global rt_task_ptr variables
bool first_cycle = true;
bool cycle12 = false;
bool cycle34 = false;
int64_t last_cycle_time = 0;
uint32_t n_errors = 0;
while (params->rt_running)
{
uint32_t pickup_flag = 0;
uint32_t spark_flag = 0;
// WAIT FOR PICKUP SIGNAL
xTaskNotifyWait(
0x00, // non pulire all'ingresso
ULONG_MAX, // pulisci i primi 8 bit
&pickup_flag, // valore ricevuto
portMAX_DELAY);
if (first_cycle && pickup_flag != TRIG_FLAG_12P) // skip first cycle because of possible initial noise on pickup signals at startu
continue;
// Start microsecond precision timeout timer
esp_timer_stop(timeout_timer); // stop timer in case it was running from previous cycle
esp_timer_start_once(timeout_timer, spark_timeout_max);
// WAIT FOR SPARK TO HAPPEN OR TIMEOUT
xTaskNotifyWait(
0x00, // non pulire all'ingresso
ULONG_MAX, // pulisci i primi 8 bit
&spark_flag, // valore ricevuto
portMAX_DELAY); // wait indefinitely, timeout handled by esp_timer
// Handle timeout or spark event
if (spark_flag != SPARK_FLAG_TIMEOUT)
esp_timer_stop(timeout_timer);
// A trigger from pickup 12 is followed by a spark event on 34 or vice versa pickup 34 triggers spark on 12
if ((pickup_flag == TRIG_FLAG_12P || pickup_flag == TRIG_FLAG_12N) && (spark_flag != SPARK_FLAG_12 && spark_flag != SPARK_FLAG_TIMEOUT))
{
ign_box_sts.coils12.spark_status = ign_box_sts.coils34.spark_status = sparkStatus::SPARK_SYNC_FAIL;
continue;
}
// Select coil status reference based on pickup_flag
coilsStatus *coils;
switch (pickup_flag)
{
case TRIG_FLAG_12P:
{
first_cycle = false;
// compute engine rpm from cycle time
auto current_time = esp_timer_get_time();
auto cycle_time = current_time - last_cycle_time;
last_cycle_time = current_time;
ign_box_sts.eng_rpm = (int32_t)(60.0f / (cycle_time / 1000000.0f));
}
case TRIG_FLAG_12N:
coils = &ign_box_sts.coils12;
break;
case TRIG_FLAG_34P:
case TRIG_FLAG_34N:
coils = &ign_box_sts.coils34;
break;
}
// Select logic based on pickup and spark flags
switch (pickup_flag)
{
case TRIG_FLAG_12P:
case TRIG_FLAG_34P:
{
// Timeout not occourred, expected POSITIVE edge spark OCCOURRED
if (spark_flag != SPARK_FLAG_TIMEOUT)
{
coils->spark_delay = (int32_t)(coils->spark_time - coils->trig_time);
coils->sstart_status = softStartStatus::NORMAL; // because spark on positive edge
coils->spark_status = sparkStatus::SPARK_POS_OK; // do not wait for spark on negative edge
}
// Timeout occourred, expected POSITIVE edge spark NOT OCCOURRED
else if (spark_flag == SPARK_FLAG_TIMEOUT)
{
coils->spark_status = sparkStatus::SPARK_NEG_WAIT;
coils->sstart_status = softStartStatus::NORMAL;
}
continue; // Do nothing more on positive pulse
}
// CASES for NEGATIVE cycle triggering of pickup and sparks 12 & 34
case TRIG_FLAG_12N:
case TRIG_FLAG_34N:
{
const bool expected_negative = coils->spark_status == sparkStatus::SPARK_NEG_WAIT;
// Timeout not occourred, expected NEGATIVE edge spark OCCOURRED
if (spark_flag != SPARK_FLAG_TIMEOUT && expected_negative)
{
coils->spark_delay = (int32_t)(coils->spark_time - coils->trig_time);
coils->sstart_status = softStartStatus::SOFT_START;
coils->spark_status = sparkStatus::SPARK_NEG_OK;
}
// Timeout occourred, expected POSITIVE edge spark NOT OCCOURRED
else if (spark_flag == SPARK_FLAG_TIMEOUT && expected_negative)
{
coils->sstart_status = softStartStatus::ERROR;
coils->spark_status = sparkStatus::SPARK_NEG_FAIL;
}
// Timeout not occouured, unexpected negative edge spark
else if (spark_flag != SPARK_FLAG_TIMEOUT && !expected_negative)
{
coils->sstart_status = softStartStatus::SOFT_START;
coils->spark_status = sparkStatus::SPARK_NEG_UNEXPECTED;
}
// Wait for finish of negative pulse to save data to buffer
coils->n_events++;
if (pickup_flag == TRIG_FLAG_12N)
cycle12 = true;
else
cycle34 = true;
break;
}
default:
break;
}
if (cycle12 && cycle34) // wait for both 12 and 34 cycles to complete before sending data to main loop and resetting peak detectors
{
// disable interrupts during adc samples
disableInterrupt(digitalPinToInterrupt(rt_int.trig_pin_12p));
disableInterrupt(digitalPinToInterrupt(rt_int.trig_pin_12n));
disableInterrupt(digitalPinToInterrupt(rt_int.trig_pin_34p));
disableInterrupt(digitalPinToInterrupt(rt_int.trig_pin_34n));
disableInterrupt(digitalPinToInterrupt(rt_int.spark_pin_12));
disableInterrupt(digitalPinToInterrupt(rt_int.spark_pin_34));
// reset coils 12 and 34 cycles
cycle12 = false;
cycle34 = false;
if (ign_box_sts.coils12.spark_status == sparkStatus::SPARK_POS_FAIL || ign_box_sts.coils12.spark_status == sparkStatus::SPARK_NEG_FAIL)
ign_box_sts.coils12.n_missed_firing++;
if (ign_box_sts.coils34.spark_status == sparkStatus::SPARK_POS_FAIL || ign_box_sts.coils34.spark_status == sparkStatus::SPARK_NEG_FAIL)
ign_box_sts.coils34.n_missed_firing++;
// read adc channels: pickup12, out12 [ pos + neg ]
if (adc) // read only if adc initialized
{
std::lock_guard<std::mutex> lock(spi_mutex);
uint32_t start_adc_read = esp_timer_get_time();
// from peak detector circuits
ign_box_sts.coils12.peak_p_in = adc->convertToVoltage(adc->cycleSingle());
ign_box_sts.coils12.peak_n_in = adc->convertToVoltage(adc->cycleSingle());
ign_box_sts.coils34.peak_p_in = adc->convertToVoltage(adc->cycleSingle());
ign_box_sts.coils34.peak_n_in = adc->convertToVoltage(adc->cycleSingle());
ign_box_sts.coils12.peak_p_out = adc->convertToVoltage(adc->cycleSingle());
ign_box_sts.coils12.peak_n_out = adc->convertToVoltage(adc->cycleSingle());
ign_box_sts.coils34.peak_p_out = adc->convertToVoltage(adc->cycleSingle());
ign_box_sts.coils34.peak_n_out = adc->convertToVoltage(adc->cycleSingle());
ign_box_sts.adc_read_time = (int32_t)(esp_timer_get_time() - start_adc_read);
adc->stopConversion();
}
else // simulate adc read timig
vTaskDelay(pdMS_TO_TICKS(c_adc_time));
// reset peak detectors + sample and hold
// outputs on io expander
if (io)
{
// Discharge Pulse
io->extDigitalWrite(rt_rst.sh_disch_12, true);
io->extDigitalWrite(rt_rst.sh_disch_34, true);
delayMicroseconds(250);
io->extDigitalWrite(rt_rst.sh_disch_12, false);
io->extDigitalWrite(rt_rst.sh_disch_34, false);
// Safety delay
delayMicroseconds(500);
// Re-Arm Pulse
io->extDigitalWrite(rt_rst.sh_arm_12, true);
io->extDigitalWrite(rt_rst.sh_arm_34, true);
delayMicroseconds(250);
io->extDigitalWrite(rt_rst.sh_arm_12, false);
io->extDigitalWrite(rt_rst.sh_arm_34, false);
}
else
vTaskDelay(pdMS_TO_TICKS(c_io_time));
// send essage to main loop with ignition info, by copy so local static variable is ok
if (rt_queue)
{
ign_box_sts.timestamp = esp_timer_get_time(); // update data timestamp
if (xQueueSendToBack(rt_queue, (void *)&ign_box_sts, 0) != pdPASS)
ign_box_sts.n_queue_errors = ++n_errors;
}
// enable interrupts ready for a new cycle
enableInterrupt(digitalPinToInterrupt(rt_int.trig_pin_12p));
enableInterrupt(digitalPinToInterrupt(rt_int.trig_pin_12n));
enableInterrupt(digitalPinToInterrupt(rt_int.trig_pin_34p));
enableInterrupt(digitalPinToInterrupt(rt_int.trig_pin_34n));
enableInterrupt(digitalPinToInterrupt(rt_int.spark_pin_12));
enableInterrupt(digitalPinToInterrupt(rt_int.spark_pin_34));
}
}
// Delete the timeout timer
esp_timer_stop(timeout_timer);
esp_timer_delete(timeout_timer);
LOG_WARN("rtTask Ending [", params->name.c_str(), "]");
// Ignition A Interrupts DETACH
detachInterrupt(rt_int.trig_pin_12p);
detachInterrupt(rt_int.trig_pin_12n);
detachInterrupt(rt_int.trig_pin_34p);
detachInterrupt(rt_int.trig_pin_34n);
detachInterrupt(rt_int.spark_pin_12);
detachInterrupt(rt_int.spark_pin_34);
// delete present task
vTaskDelete(NULL);
}
///////////// CLASS MEMBER DEFINITIONS /////////////
rtIgnitionTask::rtIgnitionTask(const rtTaskParams params, const uint32_t history_size, const uint32_t queue_size, const uint8_t core, std::mutex &fs_mutex, fs::FS &filesystem) : m_params(params), m_filesystem(filesystem), m_filesystemMutex(fs_mutex), m_core(core), m_historyMax(history_size)
{
LOG_WARN("Starting Manager for [", m_params.name.c_str(), "]");
// create queue buffers
m_rtQueueHandle = xQueueCreate(queue_size, sizeof(ignitionBoxStatus));
if (!m_rtQueueHandle)
{
LOG_ERROR("Unable To Create Task [", params.name.c_str(), "] queues");
m_managerStatus = rtTaskStatus::ERROR;
return;
}
else
m_params.rt_queue = m_rtQueueHandle;
try
{
// create PSram history vectors
m_historyBuf0 = PSHistory(history_size);
m_historyBuf1 = PSHistory(history_size);
// assing active and writable history
m_historyActive = std::unique_ptr<PSHistory>(&m_historyBuf0);
m_historyInactive = std::unique_ptr<PSHistory>(&m_historyBuf1);
}
catch (std::bad_alloc &e)
{
LOG_ERROR("Task [", params.name.c_str(), "] Unable to allocate history PSRAM: ", e.what());
return;
}
m_managerTaskName = (std::string("man_") + m_params.name).c_str();
auto task_success = xTaskCreatePinnedToCore(
rtIgnitionTask_manager,
m_managerTaskName.c_str(),
RT_TASK_STACK,
(void *)this,
m_params.rt_priority >> 2,
&m_managerHandle,
m_core);
if (task_success != pdPASS)
{
LOG_ERROR("Unable To Create Manager for [", params.name.c_str(), "]");
m_managerStatus = rtTaskStatus::ERROR;
return;
}
// average every 10 samples
m_statusFiltered = ignitionBoxStatusFiltered(m_filterSize);
m_dataLast = millis();
m_managerStatus = rtTaskStatus::OK;
}
rtIgnitionTask::~rtIgnitionTask()
{
if (m_rtHandle)
vTaskDelete(m_rtHandle);
if (m_managerHandle)
vTaskDelete(m_managerHandle);
if (m_rtQueueHandle)
vQueueDelete(m_rtQueueHandle);
}
void rtIgnitionTask::run()
{
// receive new data from the queue
auto new_data = xQueueReceive(m_rtQueueHandle, &m_statusLast, 0); // non blocking receive
if (new_data == pdPASS)
{
m_dataLast = millis();
m_managerStatus = rtTaskStatus::RUNNING;
// if history buffer is full swap buffers and if enabled save history buffer
if (m_statusCounter >= m_historyMax)
{
LOG_DEBUG("Save for Buffer Full: ", m_statusCounter);
m_statusCounter = 0;
m_savePartial = false; // reset partial save flag on new data cycle
std::swap(m_historyActive, m_historyInactive);
if (m_historySaveEnable)
saveHistory(*m_historyInactive, m_historyPath); // directly call the save task function to save without delay
LOG_INFO("Save History");
}
// update filtered data
m_statusFiltered.update(m_statusLast);
(*m_historyActive)[m_statusCounter] = m_statusLast;
// callback
if (m_onFilteredStatusUpdate && m_statusCounter % m_filterSize == 0)
{
m_onFilteredStatusUpdate(m_statusFiltered);
}
// update data counter
m_statusCounter++;
}
else
{
if (millis() - m_dataLast > c_idle_time)
{
if (m_statusCounter > 0 && !m_savePartial)
{
LOG_DEBUG("Save Partial: ", m_statusCounter);
m_historyActive->resize(m_statusCounter);
saveHistory(*m_historyActive, m_historyPath);
m_historyActive->resize(m_historyMax);
m_statusCounter = 0;
m_savePartial = true;
}
m_managerStatus = rtTaskStatus::IDLE;
}
}
}
const bool rtIgnitionTask::start()
{
LOG_WARN("Starting rtTask [", m_params.name.c_str(), "]");
auto task_success = xTaskCreatePinnedToCore(
rtIgnitionTask_realtime,
m_params.name.c_str(),
m_params.rt_stack_size,
(void *)&m_params,
m_params.rt_priority,
&m_rtHandle,
m_core);
const bool success = task_success == pdPASS && m_rtHandle != nullptr;
if (success)
m_managerStatus = rtTaskStatus::IDLE;
return success;
}
const bool rtIgnitionTask::stop()
{
LOG_WARN("Ending Task [", m_params.name.c_str(), "]");
if (m_rtHandle)
{
m_params.rt_running = false;
m_rtHandle = nullptr;
m_managerStatus = rtTaskStatus::STOPPED;
return true;
}
return false;
}
const ignitionBoxStatus rtIgnitionTask::getLast() const
{
return m_statusLast;
}
const ignitionBoxStatusFiltered rtIgnitionTask::getFiltered() const
{
return m_statusFiltered;
}
const rtIgnitionTask::rtTaskStatus rtIgnitionTask::getStatus() const
{
return m_managerStatus;
}
void rtIgnitionTask::enableSave(const bool enable, const std::filesystem::path filename)
{
m_historySaveEnable = enable;
if (enable && !filename.empty())
{
LOG_WARN("Save History Enabled Task [", m_params.name.c_str(), "]");
m_historyPath = m_filesystem.mountpoint() / filename;
}
else
{
LOG_WARN("Save History Disabled Task [", m_params.name.c_str(), "]");
}
}
void rtIgnitionTask::onMessage(std::function<void(ignitionBoxStatusFiltered)> callaback)
{
m_onFilteredStatusUpdate = callaback;
}
void rtIgnitionTask::saveHistory(const rtIgnitionTask::PSHistory &history, const std::filesystem::path &file_name)
{
// Lock filesystem mutex to avoid concurrent access
std::lock_guard<std::mutex> fs_lock(m_filesystemMutex);
// Check for free space
if (LittleFS.totalBytes() - LittleFS.usedBytes() < history.size() * sizeof(ignitionBoxStatus)) // check if at least 1MB is free for saving history
{
LOG_ERROR("Not enough space in SPIFFS to save history");
return;
}
// create complete file path
const std::filesystem::path mount_point = std::filesystem::path(m_filesystem.mountpoint());
std::filesystem::path file_path = file_name;
if (file_name.root_path() != mount_point)
file_path = mount_point / file_name;
// if firt save remove old file and create new
auto save_flags = std::ios::out;
if (m_saveFirst)
{
save_flags |= std::ios::trunc; // overwrite existing file
m_filesystem.remove(file_path.c_str()); // ensure file is removed before saving to avoid issues with appending to existing file in SPIFFS
LOG_INFO("Saving history to Flash, new file:", file_path.c_str());
}
else // else append to existing file
{
save_flags |= std::ios::app; // append to new file
LOG_INFO("Saving history to Flash, appending to existing file:", file_path.c_str());
}
std::ofstream ofs(file_path, save_flags);
if (ofs.fail())
{
LOG_ERROR("Failed to open file for writing");
return;
}
// write csv header
if (m_saveFirst)
{
ofs << "TS,EVENTS_12,DLY_12,STAT_12,V_12_1,V_12_2,V_12_3,V_12_4,IGNITION_MODE_12,"
<< "EVENTS_34,DLY_34,STAT_34,V_34_1,V_34_2,V_34_3,V_34_4,IGNITION_MODE_34,"
<< "ENGINE_RPM,ADC_READTIME,N_QUEUE_ERRORS"
<< std::endl;
ofs.flush();
m_saveFirst = false;
}
for (const auto &entry : history)
{
ofs << std::to_string(entry.timestamp) << ","
<< std::to_string(entry.coils12.n_events) << ","
<< std::to_string(entry.coils12.spark_delay) << ","
<< std::string(sparkStatusNames.at(entry.coils12.spark_status)) << ","
<< std::to_string(entry.coils12.peak_p_in) << ","
<< std::to_string(entry.coils12.peak_n_in) << ","
<< std::to_string(entry.coils12.peak_p_out) << ","
<< std::to_string(entry.coils12.peak_n_out) << ","
<< std::string(softStartStatusNames.at(entry.coils12.sstart_status)) << ","
<< std::to_string(entry.coils34.n_events) << ","
<< std::to_string(entry.coils34.spark_delay) << ","
<< std::string(sparkStatusNames.at(entry.coils34.spark_status)) << ","
<< std::to_string(entry.coils34.peak_p_in) << ","
<< std::to_string(entry.coils34.peak_n_in) << ","
<< std::to_string(entry.coils34.peak_p_out) << ","
<< std::to_string(entry.coils34.peak_n_out) << ","
<< std::string(softStartStatusNames.at(entry.coils34.sstart_status)) << ","
<< std::to_string(entry.eng_rpm) << ","
<< std::to_string(entry.adc_read_time) << ","
<< std::to_string(entry.n_queue_errors);
ofs << std::endl;
ofs.flush();
}
ofs.close();
LOG_INFO("Ignition Box history saved to Flash, records written: ", history.size());
}
Reference in New Issue View Git Blame Copy Permalink