/************************************************************************** * * Copyright (c) 2024-2024 Diality Inc. - All Rights Reserved. * * THIS CODE MAY NOT BE COPIED OR REPRODUCED IN ANY FORM, IN PART OR IN * WHOLE, WITHOUT THE EXPLICIT PERMISSION OF THE COPYRIGHT OWNER. * * @file Flow.c * * @author (last) Sean Nash * @date (last) 21-Nov-2024 * * @author (original) Sean Nash * @date (original) 21-Nov-2024 * ***************************************************************************/ #include "AlarmMgmtFP.h" #include "Flow.h" #include "FPOperationModes.h" #include "Messaging.h" #include "PersistentAlarm.h" #include "TaskPriority.h" /** * @addtogroup Flow * @{ */ // ********** private definitions ********** #define FLOWS_DATA_PUB_INTERVAL ( MS_PER_SECOND / TASK_PRIORITY_INTERVAL ) ///< Interval (ms/task time) at which the flows data is published on the CAN bus. #define DATA_PUBLISH_COUNTER_START_COUNT ( 5 ) ///< Data publish counter start count. #define FLOW_SAMPLE_FILTER_MS ( 500 ) ///< Filter flow data for given time #define FLOW_TEMP_SAMPLE_FILTER_MS ( 500 ) ///< Filter flow temperature data for given time #define SIZE_OF_FLOW_ROLLING_AVG ( FLOW_SAMPLE_FILTER_MS / TASK_PRIORITY_INTERVAL ) ///< Filtered flow moving average sample count. #define SIZE_OF_FLOW_TEMP_ROLLING_AVG ( FLOW_TEMP_SAMPLE_FILTER_MS / TASK_PRIORITY_INTERVAL ) ///< Filtered flow temprature moving average sample count. /// Defined states for the flow monitor state machine. typedef enum FlowMonitor_States { FLOW_INIT_STATE = 0, ///< Initialization state. FLOW_CONTINUOUS_READ_STATE, ///< Continuous read flow sensors state. NUM_OF_FLOW_STATES ///< Number of flow monitor states. } FLOW_STATE_T; /// Filter flow readings record. typedef struct { F32 flowReadings[ SIZE_OF_FLOW_ROLLING_AVG ]; ///< Holds flow sample rolling average. U32 flowReadingsIdx; ///< Index for next sample in rolling average array. F32 flowReadingsTotal; ///< Rolling total - used to calc average. U32 flowReadingsCount; ///< Number of samples in rolling average buffer } FILTER_FLOW_READINGS_T; /// Filter flow temperature readings record. typedef struct { F32 flowTempReadings[ SIZE_OF_FLOW_TEMP_ROLLING_AVG ]; ///< Holds pressure temperature sample rolling average. U32 flowTempReadingsIdx; ///< Index for next sample in rolling average array. F32 flowTempReadingsTotal; ///< Rolling total - used to calc average. U32 flowTempReadingsCount; ///< Number of samples in rolling average buffer } FILTER_FLOW_TEMPERATURE_READINGS_T; // ********** private data ********** static OVERRIDE_F32_T filteredcurrentFlowReadings[ NUM_OF_FLOW_SENSORS ]; ///< filtered current flow sensor flow readings (overrideable). static OVERRIDE_F32_T filteredcurrentFlowTempReadings[ NUM_OF_FLOW_SENSORS ]; ///< filtered current flow sensor temperature readings (overrideable). //static RO_PRES_SENSORS_CAL_RECORD_T flowsCalRecord; ///< Flows calibration record. static FILTER_FLOW_READINGS_T filteredFlowReadings[NUM_OF_FLOW_SENSORS]; ///< Filtered flow reading for flow sensors. static FILTER_FLOW_TEMPERATURE_READINGS_T filteredFlowTempReadings[NUM_OF_FLOW_SENSORS]; ///< Filtered temperature reading for flow sensors. static FLOW_STATE_T flowsState; ///< current state of flow monitor state machine. static U32 flowsDataPublicationTimerCounter; ///< used to schedule flow data publication to CAN bus. static OVERRIDE_U32_T flowsDataPublishInterval; ///< Flow data publish interval. // ********** private function prototypes ********** static void filterFlowSensors( void ); static void filterFlowSensorReadings( void ); static void filterFlowSensorTemperatureReadings( void ); //static F32 getCalibrationAppliedFlow( U08 sensorId, F32 flow ); static FLOW_STATE_T handleFlowsInitState( void ); static FLOW_STATE_T handleFlowsContReadState( void ); static void publishFlowsData( void ); /*********************************************************************//** * @brief * The initFlow function initializes the Flow Monitor unit. * @details \b Inputs: none * @details \b Outputs: Flow monitor unit is initialized. * @return none *************************************************************************/ void initFlow( void ) { FLOW_SENSORS_T sensor; flowsState = FLOW_INIT_STATE; flowsDataPublicationTimerCounter = DATA_PUBLISH_COUNTER_START_COUNT; // Initialize flow sensors driver initFlowSensor(); // Initialize override structures for each flow sensor for ( sensor = FLOW_SENSOR_FIRST; sensor < NUM_OF_FLOW_SENSORS; sensor++ ) { filteredcurrentFlowReadings[ sensor ].data = 0.0F; filteredcurrentFlowReadings[ sensor ].ovData = 0.0F; filteredcurrentFlowReadings[ sensor ].ovInitData = 0.0F; filteredcurrentFlowReadings[ sensor ].override = OVERRIDE_RESET; filteredcurrentFlowTempReadings[ sensor ].data = 0.0F; filteredcurrentFlowTempReadings[ sensor ].ovData = 0.0F; filteredcurrentFlowTempReadings[ sensor ].ovInitData = 0.0F; filteredcurrentFlowTempReadings[ sensor ].override = OVERRIDE_RESET; filteredFlowReadings[ sensor ].flowReadingsCount = 0; filteredFlowReadings[ sensor ].flowReadingsIdx = 0; filteredFlowReadings[ sensor ].flowReadingsTotal = 0.0F; filteredFlowTempReadings[ sensor ].flowTempReadingsCount = 0; filteredFlowTempReadings[ sensor ].flowTempReadingsIdx = 0; filteredFlowTempReadings[ sensor ].flowTempReadingsTotal = 0.0F; } flowsDataPublishInterval.data = FLOWS_DATA_PUB_INTERVAL; flowsDataPublishInterval.ovData = FLOWS_DATA_PUB_INTERVAL; flowsDataPublishInterval.ovInitData = 0; flowsDataPublishInterval.override = OVERRIDE_RESET; } /*********************************************************************//** * @brief * The filterFlowSensors function gets averages the raw flow and flow * temperature readings. * @details \b Inputs: flow and temperature readings from FPGA * @details \b Outputs: filteredFlowReadings[], filteredFlowTempReadings[] * @return none *************************************************************************/ static void filterFlowSensors( void ) { //Filter flow sensor reading filterFlowSensorReadings(); //Filter flow sensor temperature reading filterFlowSensorTemperatureReadings(); } /*********************************************************************//** * @brief * The getFilteredFlow function gets the filtered current flow (in mL/min) * for a given flow sensor. * @details \b Alarm: ALARM_ID_RO_SOFTWARE_FAULT if given sensor is invalid. * @details \b Inputs: filteredcurrentFlowReadings[] * @details \b Outputs: none * @param sensor ID of flow sensor to get filtered flow reading for. * @return The filtered current flow (in mL/min) of the given flow sensor. *************************************************************************/ F32 getFilteredFlow( FLOW_SENSORS_T sensor ) { F32 result = 0.0F; if ( sensor < NUM_OF_FLOW_SENSORS ) { result = filteredcurrentFlowReadings[ sensor ].data; if ( OVERRIDE_KEY == filteredcurrentFlowReadings[ sensor ].override ) { result = filteredcurrentFlowReadings[ sensor ].ovData; } } else { SET_ALARM_WITH_2_U32_DATA( ALARM_ID_FP_SOFTWARE_FAULT, SW_FAULT_ID_FLOW_SENSOR_INVALID_SENSOR3, (U32)sensor ) } return result; } /*********************************************************************//** * @brief * The getFilteredFlowSensorTemperature function gets the filtered current * flow sensor temperature (in deg C) for a given flow sensor. * @details \b Alarm: ALARM_ID_FP_SOFTWARE_FAULT if given sensor is invalid. * @details \b Inputs: currentFlowTempReadings * @details \b Outputs: none * @param sensor ID of flow sensor to get temperature reading for. * @return The filtered current flow sensor temperature (in deg C) of the given flow sensor. *************************************************************************/ F32 getFilteredFlowSensorTemperature( FLOW_SENSORS_T sensor ) { F32 result = 0.0F; if ( sensor < NUM_OF_FLOW_SENSORS ) { result = filteredcurrentFlowTempReadings[ sensor ].data; if ( OVERRIDE_KEY == filteredcurrentFlowTempReadings[ sensor ].override ) { result = filteredcurrentFlowTempReadings[ sensor ].ovData; } } else { SET_ALARM_WITH_2_U32_DATA( ALARM_ID_FP_SOFTWARE_FAULT, SW_FAULT_ID_FLOW_SENSOR_INVALID_SENSOR4, (U32)sensor ) } return result; } /*********************************************************************//** * @brief * The execFlowMonitor function executes the flow monitor state machine * and publishes flow data. * @note This function should be called periodically to maintain monitoring * of the flow sensors. * @details \b Inputs: flowsState * @details \b Outputs: flowsState * @details \b Alarm: ALARM_ID_FP_SOFTWARE_FAULT if in invalid flow state * @return none *************************************************************************/ void execFlowMonitor( void ) { // state machine switch ( flowsState ) { case FLOW_INIT_STATE: flowsState = handleFlowsInitState(); break; case FLOW_CONTINUOUS_READ_STATE: flowsState = handleFlowsContReadState(); break; default: SET_ALARM_WITH_2_U32_DATA( ALARM_ID_FP_SOFTWARE_FAULT, SW_FAULT_ID_FLOW_INVALID_EXEC_STATE, (U32)flowsState ) flowsState = FLOW_INIT_STATE; break; } // publish flow data on interval publishFlowsData(); } /*********************************************************************//** * @brief * The filterFlowSensorReadings function filters the flow rates for * defined interval to get average flow rates. * @details \b Inputs: filteredFlowReadings[] * @details \b Outputs: filteredFlowReadings[], filteredcurrentFlowReadings[] * @return none *************************************************************************/ static void filterFlowSensorReadings( void ) { FLOW_SENSORS_T sensor; for ( sensor = FLOW_SENSOR_FIRST; sensor < NUM_OF_FLOW_SENSORS; sensor++ ) { F32 rawFlow = (F32)getFlowRate( sensor ); // TODO - calibrate if ( filteredFlowReadings[sensor].flowReadingsCount >= SIZE_OF_FLOW_ROLLING_AVG ) { filteredFlowReadings[sensor].flowReadingsTotal -= filteredFlowReadings[sensor].flowReadings[ filteredFlowReadings[sensor].flowReadingsIdx ]; } filteredFlowReadings[sensor].flowReadings[ filteredFlowReadings[sensor].flowReadingsIdx ] = rawFlow; filteredFlowReadings[sensor].flowReadingsTotal += rawFlow; filteredFlowReadings[sensor].flowReadingsIdx = INC_WRAP( filteredFlowReadings[sensor].flowReadingsIdx, 0, SIZE_OF_FLOW_ROLLING_AVG - 1 ); filteredFlowReadings[sensor].flowReadingsCount = INC_CAP( filteredFlowReadings[sensor].flowReadingsCount, SIZE_OF_FLOW_ROLLING_AVG ); filteredcurrentFlowReadings[sensor].data = filteredFlowReadings[sensor].flowReadingsTotal / (F32)filteredFlowReadings[sensor].flowReadingsCount; } } /*********************************************************************//** * @brief * The filterPressureSensorTemperatureReadings function filters the flow sensor * temperature for defined interval to get moving average temperature reading. * @details \b Inputs: filteredFlowTempReadings[] * @details \b Outputs: filteredFlowTempReadings[], filteredcurrentFlowTempReadings[] * @return none *************************************************************************/ static void filterFlowSensorTemperatureReadings( void ) { FLOW_SENSORS_T sensor; for ( sensor = FLOW_SENSOR_FIRST; sensor < NUM_OF_FLOW_SENSORS; sensor++ ) { F32 flowTemperature = getFlowTemperature( sensor ); // TODO - calibrate? if ( filteredFlowTempReadings[sensor].flowTempReadingsCount >= SIZE_OF_FLOW_TEMP_ROLLING_AVG ) { filteredFlowTempReadings[sensor].flowTempReadingsTotal -= filteredFlowTempReadings[sensor].flowTempReadings[ filteredFlowTempReadings[sensor].flowTempReadingsIdx ]; } filteredFlowTempReadings[sensor].flowTempReadings[ filteredFlowTempReadings[sensor].flowTempReadingsIdx ] = flowTemperature; filteredFlowTempReadings[sensor].flowTempReadingsTotal += flowTemperature; filteredFlowTempReadings[sensor].flowTempReadingsIdx = INC_WRAP( filteredFlowTempReadings[sensor].flowTempReadingsIdx, 0, SIZE_OF_FLOW_TEMP_ROLLING_AVG - 1 ); filteredFlowTempReadings[sensor].flowTempReadingsCount = INC_CAP( filteredFlowTempReadings[sensor].flowTempReadingsCount, SIZE_OF_FLOW_TEMP_ROLLING_AVG ); filteredcurrentFlowTempReadings[sensor].data = filteredFlowTempReadings[sensor].flowTempReadingsTotal / (F32)filteredFlowTempReadings[sensor].flowTempReadingsCount; } } /*********************************************************************//** * @brief * The getCalibrationAppliedFlow function applies the calibration values * to a given flow sensor and filtered flow rate and returns a calibrated flow. * @details \b Inputs: flowsCalRecord * @details \b Outputs: none * @param sensorId the ID of the flow sensor * @param flow the flow rate before applying calibration to it * @return calibration applied flow rate *************************************************************************/ //static F32 getCalibrationAppliedFlow( U08 sensorId, F32 flow ) //{ // F32 calFlow = pow( flow, 4 ) * flowsCalRecord.flowSensors[ (FLOW_SENSORS_T)sensorId ].fourthOrderCoeff + // pow( flow, 3 ) * flowsCalRecord.flowSensors[ (FLOW_SENSORS_T)sensorId ].thirdOrderCoeff + // pow( flow, 2 ) * flowsCalRecord.flowSensors[ (FLOW_SENSORS_T)sensorId ].secondOrderCoeff + // flow * flowsCalRecord.flowSensors[ (FLOW_SENSORS_T)sensorId ].gain + // flowsCalRecord.flowSensors[ (FLOW_SENSORS_T)sensorId ].offset; // return calFlow; //} /*********************************************************************//** * @brief * The handleFlowsInitState function handles the flows initialize state * of the flow monitor state machine. * @details \b Inputs: none * @details \b Outputs: none * @return next state *************************************************************************/ static FLOW_STATE_T handleFlowsInitState( void ) { return FLOW_CONTINUOUS_READ_STATE; } /*********************************************************************//** * @brief * The handleFlowsContReadState function handles the continuous read state * of the flows monitor state machine. Raw readings are read from driver * and then filtered via moving average and calibrated. * @details \b Inputs: raw readings from flow sensor driver * @details \b Outputs: flow sensors are sampled, filtered and calibrated. * @return next state *************************************************************************/ static FLOW_STATE_T handleFlowsContReadState( void ) { FLOW_STATE_T result = FLOW_CONTINUOUS_READ_STATE; // Get raw flow readings readFlowSensors(); // filter raw flow readings filterFlowSensors(); return result; } /*********************************************************************//** * @brief * The publishFlowsData function publishes FP flow data at a set interval. * @details \b Inputs: flowsDataPublicationTimerCounter * @details \b Outputs: flowsDataPublicationTimerCounter * @details \b Message \b Sent: MSG_ID_FP_FLOW_DATA to publish flow data. * @return none *************************************************************************/ static void publishFlowsData( void ) { // publish pressure/occlusion data on interval if ( ++flowsDataPublicationTimerCounter >= getU32OverrideValue( &flowsDataPublishInterval ) ) { FLOW_TEMP_DATA_T data; data.p7Flow = getFilteredFlow( P7_FLOW ); data.p16Flow = getFilteredFlow( P16_FLOW ); data.p7Temp = getFilteredFlowSensorTemperature( P7_FLOW ); data.p16Temp = getFilteredFlowSensorTemperature( P16_FLOW ); broadcastData( MSG_ID_FP_FLOW_DATA, COMM_BUFFER_OUT_CAN_FP_BROADCAST, (U08*)&data, sizeof( FLOW_TEMP_DATA_T ) ); flowsDataPublicationTimerCounter = 0; } } /************************************************************************* * TEST SUPPORT FUNCTIONS *************************************************************************/ /*********************************************************************//** * @brief * The testFlowSensorDataPublishIntervalOverride function overrides the * flow sensor data publish interval. * @details \b Inputs: none * @details \b Outputs: flowsDataPublishInterval * @param message Override message from Dialin which includes the value * that override flow data publish interval with (in ms) * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testFlowSensorDataPublishIntervalOverride( MESSAGE_T *message ) { BOOL result = u32BroadcastIntervalOverride( message, &flowsDataPublishInterval, TASK_PRIORITY_INTERVAL ); return result; } /*********************************************************************//** * @brief * The testFlowSensorFilteredReadingsOverride function overrides the * filtered value of the specified flow sensor with a given value. * @details \b Inputs: none * @details \b Outputs: filteredcurrentFlowReadings[] * @param message Override message from Dialin which includes a sensor * ID and override value of the flow rate for that sensor. * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testFlowSensorFilteredReadingsOverride( MESSAGE_T *message ) { BOOL result = f32ArrayOverride( message, &filteredcurrentFlowReadings[0], NUM_OF_FLOW_SENSORS - 1 ); return result; } /*********************************************************************//** * @brief * The testFlowSensorFilteredTemperatureReadingsOverride function overrides the * value of the specified flow sensor filtered temperature with a given value. * @details \b Inputs: none * @details \b Outputs: filteredcurrentFlowTempReadings[] * @param message Override message from Dialin which includes a sensor * ID and override value of the flow sensor temperature. * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testFlowSensorFilteredTemperatureReadingsOverride( MESSAGE_T *message ) { BOOL result = f32ArrayOverride( message, &filteredcurrentFlowTempReadings[0], NUM_OF_FLOW_SENSORS - 1 ); return result; } /**@}*/