diff --git a/src/modules/control.cpp b/src/modules/control.cpp index d2e8634..9efa279 100644 --- a/src/modules/control.cpp +++ b/src/modules/control.cpp @@ -29,10 +29,12 @@ #define INHALATION_OVERTIME(t) ((t) * 4 / 3) -#define MAX_PEAK_PRESSURE 40.0f +#define MAX_PEAK_PRESSURE ((float)parameters.highPressureLimit/100.0f) #define CONTROL_INITIAL_PHASESHIFT_MS 200L +#define VIH_BREATH_THRESHOLD 20 + // Uncomment the following to enable the current closed loop control #define CONTROL_CLOSED_LOOP @@ -148,9 +150,10 @@ static int32_t inhalationTrajectoryPhaseShiftEstimate=CONTROL_INITIAL_PHASESHIFT static uint8_t inhalationTrajectoryInitialCycleCnt=0; static uint8_t controlForceHome=0; static uint8_t controlLimitHit=CONTROL_LIMIT_HIT_NONE; +static int32_t patientSynchronyCorrectionTime; -#define INITIAL_FLOW_SAFETY_FACTOR 0.5f //start unmeasured ventilation at lower value and rather increase over time than to deliver too much +#define INITIAL_FLOW_SAFETY_FACTOR 0.7f //start unmeasured ventilation at lower value and rather increase over time than to deliver too much float initialFlow() { float timeInS=(float)targetInhalationTime/(float)(1.0f SEC); @@ -298,9 +301,11 @@ static int generateFlowInhalationTrajectory(uint8_t init) inhalationTrajectoryState=STATE_INHALATION_TRAJECTORY_INIT; // Collect all set points from parameters totalBreathTime = (60 SEC) / parameters.respirationRateRequested; + targetInhalationTime = (parameters.ieRatioRequested * totalBreathTime) >> 8; // TODO: address fixed point math targetInhalationTime-= HOLD_TIME; //HOLD is part of inhalation targetInhalationTime+=inhalationTrajectoryPhaseShiftEstimate; //allow more time to compensate for initial flow buildup in the bag as measure by the control loop + patientSynchronyCorrectionTime=0; targetHoldTime = HOLD_TIME; // fixed value @@ -329,15 +334,13 @@ static int generateFlowInhalationTrajectory(uint8_t init) if (controlLimitHit==CONTROL_LIMIT_HIT_PRESSURE) { //we hit a limit, make a down adjustment of the current trajectory and don't change it for 2 more cycles - inhalationTrajectoryStartFlow*=0.95; - inhalationTrajectoryEndFlow*=0.95; - inhalationTrajectoryInitialCycleCnt=1; + inhalationTrajectoryStartFlow*=0.90f; + inhalationTrajectoryEndFlow*=0.90f; controlLimitHit=CONTROL_LIMIT_GOT_HANDLED; }else if (controlLimitHit==CONTROL_LIMIT_HIT_VOLUME) { - inhalationTrajectoryStartFlow*=0.99; - inhalationTrajectoryEndFlow*=0.99; - inhalationTrajectoryInitialCycleCnt=1; + inhalationTrajectoryStartFlow*=0.95f; + inhalationTrajectoryEndFlow*=0.95f; controlLimitHit=CONTROL_LIMIT_GOT_HANDLED; } @@ -352,6 +355,13 @@ static int generateFlowInhalationTrajectory(uint8_t init) { // Collect all set points from parameters totalBreathTime = (60 SEC) / parameters.respirationRateRequested; + totalBreathTime-=patientSynchronyCorrectionTime; + + //reduce 20% + patientSynchronyCorrectionTime*=8; + patientSynchronyCorrectionTime/=10; + + targetInhalationTime = (parameters.ieRatioRequested * totalBreathTime) >> 8; // TODO: address fixed point math targetInhalationTime-= HOLD_TIME; //HOLD is part of inhalation targetInhalationTime+=inhalationTrajectoryPhaseShiftEstimate; //allow more time to compensate for initial flow buildup in the bag as measure by the control loop @@ -668,7 +678,7 @@ static int updateControl(void) controlLogData.targetHoldTime=targetHoldTime; controlLogData.targetInhalationTime=targetInhalationTime; controlLogData.controlLimitHit=controlLimitHit; - controlLogData.controlI=controlI; + controlLogData.controlI=sensors.virtualInhalationSensor; controlLogData.currentFlow=sensors.currentFlow; controlLogData.currentPressure=sensors.currentPressure; @@ -696,6 +706,31 @@ static bool checkExhalationTimeout(void) return (timerHalRun(&breathTimer) != HAL_IN_PROGRESS); } +static bool checkForPatientBreath(void) +{ + if (control.allowPatientSync==0) + return true; + //now is the time to potentially sync to a patients breath + if ((sensors.virtualInhalationSensor>VIH_BREATH_THRESHOLD) && (controlLimitHit==CONTROL_LIMIT_HIT_NONE)) //don't adapt in control limit hit cycles + { + //Patient is try to breathe, calculate a shift in time to sync + uint32_t patientBreathTime=timerHalCurrent(&breathTimer); + + LOG_PRINT(DEBUG,"Patient Breath detected at %li of time %li",patientBreathTime,breathTimer.duration); + + //reduce duration, but not fully to sync slowly to the patient and don't screw up on false readings + patientSynchronyCorrectionTime=(breathTimer.duration-patientBreathTime); + + if (patientSynchronyCorrectionTime<0) + patientSynchronyCorrectionTime=0; + breathTimer.duration-=patientSynchronyCorrectionTime; + + control.allowPatientSync=0; + return true; + } + return false; +} + static PT_THREAD(controlThreadMain(struct pt* pt)) { PT_BEGIN(pt); @@ -836,11 +871,12 @@ static PT_THREAD(controlThreadMain(struct pt* pt)) // Begin control loop timer when control starts timerHalBegin(&controlTimer, CONTROL_LOOP_PERIOD, true); + control.allowPatientSync=1; while (1) { // TODO: Different modes? controlComplete = updateControl(); - + checkForPatientBreath(); // If the control still hasnt reached its destination and the timeout // condition hasnt been met, continue the control loop, waiting for the // next control cycle; otherwise move on to the next steps in exhalation @@ -852,6 +888,9 @@ static PT_THREAD(controlThreadMain(struct pt* pt)) break; } } + + control.outputFiltered=0.0f; + motorHalCommand(MOTOR_HAL_COMMAND_HOLD, 0); @@ -859,7 +898,7 @@ static PT_THREAD(controlThreadMain(struct pt* pt)) // At this point, either wait for the breath timer to expire or find a new // breath to sync with - PT_WAIT_UNTIL(pt, ((timerHalRun(&breathTimer) != HAL_IN_PROGRESS))); + PT_WAIT_UNTIL(pt, ((timerHalRun(&breathTimer) != HAL_IN_PROGRESS) || checkForPatientBreath())); // Calculate and update the measured times uint32_t currentBreathTime = timerHalCurrent(&breathTimer); @@ -871,6 +910,7 @@ static PT_THREAD(controlThreadMain(struct pt* pt)) LOG_PRINT(INFO, "Timing report [ms]: It: %li Ps: %li Et: %li I:E [1=1000]: %li RR:%li",(uint32_t)measuredInhalationTime/(uint32_t)1000, (uint32_t)inhalationTrajectoryPhaseShiftEstimate/(uint32_t)1000, (uint32_t)measuredExhalationTime/1000, (uint32_t)measuredInhalationTime*(uint32_t)1000/(uint32_t)measuredExhalationTime, (uint32_t)control.respirationRateMeasured); // Check if we need to continue onto another breath or if ventilation has stopped + control.allowPatientSync=0; if (controlForceHome) { control.state = CONTROL_STATE_HOME; @@ -888,6 +928,8 @@ static PT_THREAD(controlThreadMain(struct pt* pt)) // TODO: Error, unknown control state!!! control.state = CONTROL_STATE_IDLE; } + + control.outputFiltered=controlOutputFiltered; PT_YIELD(pt); } @@ -902,6 +944,8 @@ int controlModuleInit(void) return MODULE_FAIL; } + control.outputFiltered=0.0f; + PT_INIT(&controlThread); return MODULE_OK; diff --git a/src/modules/control.h b/src/modules/control.h index d2c13fc..a5dd2d7 100644 --- a/src/modules/control.h +++ b/src/modules/control.h @@ -25,10 +25,12 @@ struct control { uint32_t respirationRateMeasured; uint32_t ieRatioMeasured; uint32_t breathCount; - + float outputFiltered; // Alarms int8_t breathTimeoutAlarm; int8_t unknownStateAlarm; + + uint8_t allowPatientSync; }; // Public Variables diff --git a/src/modules/sensors.cpp b/src/modules/sensors.cpp index 17d2237..d909b1d 100644 --- a/src/modules/sensors.cpp +++ b/src/modules/sensors.cpp @@ -486,6 +486,38 @@ static PT_THREAD(sensorsAirFlowThreadMain(struct pt* pt)) airflowResponseCount=-1; } + static int32_t vihValue; + static int32_t lastPressure; + static int32_t lastFlow; + static int32_t lastControlOutput; + if (control.allowPatientSync) + { + vihValue=sensors.peepPressure-sensors.currentPressure; //negative pressure + if (vihValue<0) + vihValue=0; + + if (vihValue>500) + vihValue=500; + + vihValue*=(lastPressure-sensors.currentPressure); + vihValue/=50; //implicit *10 + + if (vihValue<0) + vihValue=0; + sensors.virtualInhalationSensor=(7*sensors.virtualInhalationSensor+3*vihValue)/10; + }else + { + vihValue=0; + sensors.virtualInhalationSensor=0; + } + + + + lastPressure=sensors.currentPressure; + lastFlow=sensors.currentFlow; + + sensors.volumeOut=sensors.virtualInhalationSensor; + LOG_PRINT_EVERY(200, INFO, "Airflow = %c%u.%02u SLM", (airflow < 0) ? '-' : ' ', abs(airflow)/100, abs(airflow)%100); // DEBUG_PRINT_EVERY(200, "AirVolume = %d mL", airvolume); @@ -558,6 +590,8 @@ int sensorsModuleInit(void) alarmInit(&sensors.highRespiratoryRateAlarm, &highRespiratoryRateAlarmProperties); alarmInit(&sensors.lowRespiratoryRateAlarm, &lowRespiratoryRateAlarmProperties); + sensors.virtualInhalationSensor=0; + PT_INIT(&sensorsThread); PT_INIT(&sensorsPressureThread); PT_INIT(&sensorsAirFlowThread); diff --git a/src/modules/sensors.h b/src/modules/sensors.h index 1f4306a..dc57c55 100644 --- a/src/modules/sensors.h +++ b/src/modules/sensors.h @@ -19,6 +19,7 @@ struct sensors { int32_t peakPressure; int32_t plateauPressure; int32_t peepPressure; + int32_t virtualInhalationSensor; bool inhalationDetected; bool exhalationDetected;