From 20ec6ecf17658bda7a3657f9b66c6bd3546a942e Mon Sep 17 00:00:00 2001 From: "Rosen, Michael R" Date: Mon, 6 Apr 2020 18:05:38 -0700 Subject: [PATCH 01/15] control: flesh out control fsm Further refine high-level controller and make clearer the separation between high- and mid-level control loops Signed-off-by: Rosen, Michael R --- src/modules/control.cpp | 124 ++++++++++++++++++++++++++++++++-------- src/modules/control.h | 2 +- src/modules/link.cpp | 7 +-- 3 files changed, 102 insertions(+), 31 deletions(-) diff --git a/src/modules/control.cpp b/src/modules/control.cpp index ceea1df..39f0b6e 100644 --- a/src/modules/control.cpp +++ b/src/modules/control.cpp @@ -2,6 +2,8 @@ // Control Module // +#include + #include "../pt/pt.h" #include "../hal/motor.h" @@ -9,12 +11,16 @@ #include "../modules/module.h" #include "../modules/control.h" +#include "../modules/sensors.h" #include "../modules/parameters.h" // #define DEBUG #define DEBUG_MODULE "control" #include "../util/debug.h" +#define INHALATION_OVERTIME 4 / 3 +#define HOLD_TIME (200 MSEC) + // Public Variables struct control control = { .state = CONTROL_IDLE @@ -22,10 +28,28 @@ struct control control = { // Private Variables static struct pt controlThread; +static struct timer breathTimer; static struct timer controlTimer; -static unsigned int motorCompressionDistance; -static unsigned int motorCompressionDuration; +static uint16_t totalMotorCompressionDistance; +static uint16_t totalMotorCompressionDuration; + +static uint16_t targetVolume; +static uint32_t totalBreathTime; +static uint32_t targetInhalationTime; + +static uint32_t measuredInhalationTime; +static uint32_t measuredExhalationTime; + +// Mid-level volume control function +static void updateVolumeControl(uint16_t* distance, uint16_t* duration) +{ + // TODO: fill-in this function; for now just perform a 40 degree motion + if (totalMotorCompressionDistance == 0) { + *distance = 40; + *duration = 0; + } +} static PT_THREAD(controlThreadMain(struct pt* pt)) { @@ -40,41 +64,72 @@ static PT_THREAD(controlThreadMain(struct pt* pt)) if (control.state == CONTROL_IDLE) { DEBUG_PRINT_EVERY(10000, "state: CONTROL_IDLE"); + // Reset any parameters + control.breathCount = 0; + control.ieRatioMeasured = 0; + control.respirationRateMeasured = 0; + // Wait for the parameters to enter the run state before - if (parameters.startVentilation) { - control.state = CONTROL_BEGIN_INHALATION; - } + PT_WAIT_UNTIL(pt, parameters.startVentilation); + + control.state = CONTROL_BEGIN_INHALATION; } else if (control.state == CONTROL_BEGIN_INHALATION) { DEBUG_PRINT("state: CONTROL_BEGIN_INHALATION"); - // if (parameters.ventilationMode == VENTILATOR_MODE_VC) { - if (true) { - // TODO: Calculate breath parameters and motor control. - motorCompressionDistance = 40; // Fixed to 40 degrees excursion for now. - motorCompressionDuration = 0; - // TODO: Error check - motorHalBegin(MOTOR_HAL_DIRECTION_INHALATION, motorCompressionDistance, motorCompressionDuration); - } else { - // TODO: Implement Assist mode setup - } + // Collect all set points from parameters + totalBreathTime = (60 SEC) / parameters.respirationRateRequested; + targetInhalationTime = (parameters.ieRatioRequested * totalBreathTime) >> 8; + targetVolume = parameters.volumeRequested; + + // Initialize all + totalMotorCompressionDistance = 0; + totalMotorCompressionDuration = 0; + measuredInhalationTime = 0; + measuredExhalationTime = 0; + + // Kickoff timers for monitoring breath/stages of breathing + timerHalBegin(&breathTimer, totalBreathTime); + timerHalBegin(&controlTimer, targetInhalationTime * INHALATION_OVERTIME); + control.state = CONTROL_INHALATION; } else if (control.state == CONTROL_INHALATION) { DEBUG_PRINT("state: CONTROL_INHALATION"); - // if (parameters.ventilationMode == VENTILATOR_MODE_VC) { - if (true) { - PT_WAIT_UNTIL(pt, motorHalRun() != HAL_IN_PROGRESS); + uint16_t motorDistance = 0; + uint16_t motorDuration = 0; + + // If in a volume controlled mode, run the Mid-level volume controller + if ((parameters.ventilationMode == VENTILATOR_MODE_VC) || + (parameters.ventilationMode == VENTILATOR_MODE_AC)) { + + // Mid-level controller function + updateVolumeControl(&motorDistance, &motorDuration); + } + + // Update distance/duration accumulators (for exhalation) + totalMotorCompressionDistance += motorDistance; + totalMotorCompressionDuration += motorDuration; + + // If the controller reports zero distance or the timer has expired, move + // on to next state, otherwi< Date: Wed, 22 Apr 2020 16:31:52 -0700 Subject: [PATCH 02/15] Refactored motor driver for glitch-free PWM output. --- src/hal/motor.cpp | 254 +++++++++++++++++++++++++++++++++++----------- 1 file changed, 197 insertions(+), 57 deletions(-) diff --git a/src/hal/motor.cpp b/src/hal/motor.cpp index 0ffff0f..20b75ac 100644 --- a/src/hal/motor.cpp +++ b/src/hal/motor.cpp @@ -24,6 +24,9 @@ #define PIN_MOTOR_ENABLE 8 // Enable locks the motor in place when it is not moving, which consumes power. #define PIN_MOTOR_DIRECTION 9 +#define PIN_MOTOR_ENABLE_FALSE LOW +#define PIN_MOTOR_ENABLE_TRUE HIGH + #define PIN_MOTOR_DIRECTION_OPEN LOW #define PIN_MOTOR_DIRECTION_CLOSE HIGH @@ -74,14 +77,21 @@ #define MC_MICROSTEP_RESOLUTION 400 #define MC_MICROSTEPS_PER_STEP (MC_MICROSTEP_RESOLUTION/100) +// TODO +// Add definitions for other motor controllers. + //************************************** -// Motor Control Definitions +// Motor State Machine Definitions //************************************** #define MOTOR_STATE_OFF 0 #define MOTOR_STATE_HOLD 1 #define MOTOR_STATE_OPEN 2 #define MOTOR_STATE_CLOSE 3 +#define MOTOR_STATE_NONE UINT8_MAX +//************************************** +// Motor Control Definitions +//************************************** #define MOTOR_DIRECTION_OPEN -1 #define MOTOR_DIRECTION_CLOSE 1 @@ -106,10 +116,13 @@ static volatile struct { int8_t microstep_position = 0; } motor_control; -// Motor State Machine +//************************************** +// Motor State Machine Variables +//************************************** static volatile uint8_t motor_state = MOTOR_STATE_OFF; +static volatile uint8_t motor_state_pending = MOTOR_STATE_NONE; -static volatile bool motor_busy = false; +static volatile bool motor_busy = false; // TODO: Remove //***************************************************************************** // Timer 3 Configuration: Motor State Control @@ -124,7 +137,7 @@ void timer3_setup() { TCNT3 = 0; // OCR3A = 20000; // 50 Hz (1,000,000 / 20,000 = 50) OCR3A = 10000; // 100 Hz (1,000,000 / 10,000 = 100) - TIMSK3 = (1< ATMega 2560 PH4 -> Arduino D7) +// COM4B[1:0] = 0b10 (Enable non-inverting PWM output on OC4B -> ATMega 2560 PH4 -> Arduino D7) // CS3[2:0] = 0b010 (clk/8 prescaler) // // Create values for the control registers such that the timer can be enabled // and disabled dynamically. //***************************************************************************** static uint8_t TCCR4A_value = (1< MOTOR_CONTROL_STEP_ERROR) { if (digitalRead(PIN_LIMIT_BOTTOM) == PIN_LIMIT_TRIPPED) { // Do not move the motor in the CLOSE direction if the bottom limit switch is tripped. - set_motor_state(MOTOR_STATE_HOLD); + motor_state_set(MOTOR_STATE_HOLD); } else { - set_motor_state(MOTOR_STATE_CLOSE); + motor_state_set(MOTOR_STATE_CLOSE); } } else if (motor_step_delta < -MOTOR_CONTROL_STEP_ERROR) { if (digitalRead(PIN_LIMIT_TOP) == PIN_LIMIT_TRIPPED) { // Do not move the motor in the OPEN direction if the top limit switch is tripped. - set_motor_state(MOTOR_STATE_HOLD); + motor_state_set(MOTOR_STATE_HOLD); motor_position_zero(); } else { - set_motor_state(MOTOR_STATE_OPEN); + motor_state_set(MOTOR_STATE_OPEN); } } else { if ((motor_state == MOTOR_STATE_OPEN) || (motor_state == MOTOR_STATE_CLOSE)) { // Hold the motor in place in order to maintain the correct position // without having to zero the motor again. - set_motor_state(MOTOR_STATE_HOLD); + motor_state_set(MOTOR_STATE_HOLD); } } } // Motor Position Tracking ISR -static inline void motor_position_tracking_ISR() +static inline void motor_position_update() { if (motor_state == MOTOR_STATE_OPEN) { if (--motor_control.microstep_position <= -MC_MICROSTEPS_PER_STEP) { @@ -343,19 +469,33 @@ static inline void motor_position_tracking_ISR() motor_control.step_position++; motor_control.microstep_position = 0; } + } else { + // TODO: error } } -ISR(TIMER3_COMPA_vect) +// Timer 3 BOTTOM +ISR(TIMER3_OVF_vect) { - motor_state_control_ISR(); + motor_state_update(); +} + +// Timer 4 BOTTOM +// This routine is used for changing the motor from a moving state to a stopped +// state. +ISR(TIMER4_OVF_vect) { + motor_state_install_pending(); } +// Timer 4 TOP ISR(TIMER4_COMPA_vect) { - motor_position_tracking_ISR(); + motor_position_update(); + + // TODO: Install pending motor PWM update command. } + //***************************************************************************** // motorHal Interface Implementation //***************************************************************************** From 9a3cc088b60aaf678a2435c31f0446f579332f50 Mon Sep 17 00:00:00 2001 From: Jonner Steck Date: Wed, 22 Apr 2020 23:31:35 -0700 Subject: [PATCH 03/15] Updated motor driver with speed control. Moved motor state control out of ISR. --- src/hal/motor.cpp | 234 +++++++++++++++++++++------------------- src/hal/motor.h | 13 ++- src/modules/control.cpp | 43 +++----- 3 files changed, 142 insertions(+), 148 deletions(-) diff --git a/src/hal/motor.cpp b/src/hal/motor.cpp index 20b75ac..94177a5 100644 --- a/src/hal/motor.cpp +++ b/src/hal/motor.cpp @@ -1,9 +1,7 @@ - #include "../hal/hal.h" #include "../hal/motor.h" #include -#include #include #include "Arduino.h" @@ -11,7 +9,7 @@ #include #include -// #define DEBUG +#define DEBUG #define DEBUG_MODULE "motor" #include "../util/debug.h" @@ -43,6 +41,8 @@ // Motor Definitions //************************************** +#define MOTOR_STEPS_PER_REVOLUTION 200 + // millidegrees of shaft rotation (including the gearbox) per motor step // stepperonline 23HS22-2804S-HG50 @@ -77,8 +77,19 @@ #define MC_MICROSTEP_RESOLUTION 400 #define MC_MICROSTEPS_PER_STEP (MC_MICROSTEP_RESOLUTION/100) -// TODO -// Add definitions for other motor controllers. +// stepperonline DM332T +// Pulse/rev +// The DM332T supports the following settings: +// 400 (half steps) +// 800 (quarter steps) +// 1600 (eigth steps) +// 3200 (sixteenth steps) +// 4000 +// 6400 +// 8000 +// 12800 +// #define MC_MICROSTEPS_PER_REVOLUTION 800 +// #define MC_MICROSTEPS_PER_STEP (MC_MICROSTEPS_PER_REVOLUTION/MOTOR_STEPS_PER_REVOLUTION) //************************************** // Motor State Machine Definitions @@ -92,28 +103,22 @@ //************************************** // Motor Control Definitions //************************************** -#define MOTOR_DIRECTION_OPEN -1 -#define MOTOR_DIRECTION_CLOSE 1 -// TODO -// This value should be derived from Timer 3 and Timer 4 frequencies. +// TODO: Update this based on maximum PWM frequency and control loop frequency. #define MOTOR_CONTROL_STEP_ERROR 5 //************************************** // Motor Control Variables //************************************** - -// Staging area for motor move commands. -static struct { - bool valid = false; - uint8_t direction = 0; - uint8_t distance = 0; -} motor_move_command; - static volatile struct { - int16_t step_command = 0; - int16_t step_position = 0; - int8_t microstep_position = 0; + // Precomputed value so that we can minimize the amount of math operations + // inside of an ISR. + bool counter_update; + uint16_t counter_TOP; + + int16_t step_position_command; + int16_t step_position; + int8_t microstep_position; } motor_control; //************************************** @@ -122,8 +127,6 @@ static volatile struct { static volatile uint8_t motor_state = MOTOR_STATE_OFF; static volatile uint8_t motor_state_pending = MOTOR_STATE_NONE; -static volatile bool motor_busy = false; // TODO: Remove - //***************************************************************************** // Timer 3 Configuration: Motor State Control // @@ -167,7 +170,7 @@ void timer4_setup() TCCR4B = TCCR4B_value_disabled; TCNT4 = 0; - // TODO + // TODO: Set defaults. OCR4A = 625; OCR4B = OCR4A/2; // 50% duty cycle @@ -180,33 +183,34 @@ static inline void timer4_enable() { static inline void timer4_disable() { TCCR4B = TCCR4B_value_disabled; + TCNT4 = 0; // TODO: Does TCNT need to be written to zero after OCR4A/OCR4B get written in order to latch in their values? } static inline void timer4_interrupt_BOTTOM_enable() { - TIMSK4 |= _BV(TOIE4); + TIFR4 = _BV(TOV4); // Clear the Timer 4 Overflow Flag. + TIMSK4 |= _BV(TOIE4); // Set the Timer 4 Overflow Interrupt Enable. } static inline void timer4_interrupt_BOTTOM_disable() { - TIMSK4 &= ~_BV(TOIE4); + TIMSK4 &= ~_BV(TOIE4); // Clear the Timer 4 Overflow Interrupt Enable. } -/* -void timer4_update_frequency() +static inline void timer4_set_TOP(uint16_t counter_value) { - + OCR4A = counter_value; + OCR4B = counter_value/2; // 50% duty cycle } -*/ //***************************************************************************** // Motor PWM Control //***************************************************************************** -// TODO: Set up frequency. -// TODO: Clear timer. static inline void motor_pwm_enable() { + timer4_set_TOP(motor_control.counter_TOP); + motor_control.counter_update = false; timer4_enable(); } @@ -233,30 +237,29 @@ static inline void motor_pwm_phase_interrupt_disable() // its outputs accordingly. static void motor_state_set_OFF() { + DEBUG_PRINT("MOTOR_STATE_OFF"); motor_pwm_disable(); digitalWrite(PIN_MOTOR_ENABLE, PIN_MOTOR_ENABLE_FALSE); motor_state = MOTOR_STATE_OFF; - - motor_busy = false; } // Sets the motor state machine to the MOTOR_STATE_HOLD state, and configures // its outputs accordingly. static void motor_state_set_HOLD() { + DEBUG_PRINT("MOTOR_STATE_HOLD"); motor_pwm_disable(); digitalWrite(PIN_MOTOR_ENABLE, PIN_MOTOR_ENABLE_TRUE); motor_state = MOTOR_STATE_HOLD; - - motor_busy = false; } // Sets the motor state machine to the MOTOR_STATE_OPEN state, and configures // its outputs accordingly. static inline void motor_state_set_OPEN() { + DEBUG_PRINT("MOTOR_STATE_OPEN"); digitalWrite(PIN_MOTOR_DIRECTION, PIN_MOTOR_DIRECTION_OPEN); digitalWrite(PIN_MOTOR_ENABLE, PIN_MOTOR_ENABLE_TRUE); motor_pwm_enable(); @@ -268,6 +271,7 @@ static inline void motor_state_set_OPEN() // its outputs accordingly. static inline void motor_state_set_CLOSE() { + DEBUG_PRINT("MOTOR_STATE_CLOSE"); digitalWrite(PIN_MOTOR_DIRECTION, PIN_MOTOR_DIRECTION_CLOSE); digitalWrite(PIN_MOTOR_ENABLE, PIN_MOTOR_ENABLE_TRUE); motor_pwm_enable(); @@ -317,12 +321,12 @@ static bool motor_state_moving() // // A transition from a moving state directly to another moving state is treated // as an error. -void motor_state_set(uint8_t next_state) +void motor_state_transition(uint8_t next_state) { if (motor_state == next_state) { return; } - + if (motor_state_pending != MOTOR_STATE_NONE) { if (next_state != motor_state_pending) { // TODO: error @@ -365,34 +369,23 @@ void motor_state_set(uint8_t next_state) //***************************************************************************** // Set the current position as the zero point. -void motor_position_zero() +void motor_position_set_zero() { assert((motor_state != MOTOR_STATE_OPEN) && (motor_state != MOTOR_STATE_CLOSE)); - motor_control.microstep_position = 0; + motor_control.step_position_command = 0; motor_control.step_position = 0; - motor_control.step_command = 0; + motor_control.microstep_position = 0; } -// Moves the motor to an angle relative to its current position. +// Sets the motor command positon. // -// Parameters -// ---------- -// uint8_t angle The angle (in degrees) to move the motor. -// int8_t direction The direction to move the motor. One of: -// * MOTOR_DIRECTION_OPEN -// * MOTOR_DIRECTION_CLOSE -void motor_move(uint8_t angle, int8_t direction) +// Parameter +// --------- +// uint8_t position The absolute position to which to move the motor. +// units: degrees +void motor_position_set(int8_t position) { - // TODO: Do we want to have assertions? How are they implemented in this - // system? (i.e. Do they cause a watchdog reset, print a debug message to the - // console, etc.) - assert((direction == MOTOR_DIRECTION_OPEN) || - (direction == MOTOR_DIRECTION_CLOSE)); - - ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { - motor_busy = true; - // Deriving motor_control.step_command from motor_control.step_position can // cause error to accumulate from movement to movement. If @@ -401,8 +394,29 @@ void motor_move(uint8_t angle, int8_t direction) // motor_control.step_command = motor_control.step_position + direction*((angle*1000)/MOTOR_STEP_ANGLE); // Convert to millidegrees and account for direction. - motor_control.step_command += (int16_t) (direction*((((int32_t) angle)*1000)/MOTOR_STEP_ANGLE)); - } + motor_control.step_position_command = (int16_t) ((((int32_t) position)*1000)/MOTOR_STEP_ANGLE); + +} + +//***************************************************************************** +// Motor Speed Control +//***************************************************************************** + +// Sets the motor command speed. +// +// Parameter +// --------- +// uint8_t speed The speed at which to move the motor. +// units: MPRM (milli-RPM) +void motor_speed_set(uint16_t speed) +{ + // Convert from MRPM to frequency. + uint32_t frequency = (((((uint32_t) speed)*360U)/60U)/((uint8_t) MOTOR_STEP_ANGLE))*((uint8_t) MC_MICROSTEPS_PER_STEP); + + // Convert from frequency to counter value. + motor_control.counter_TOP = (uint16_t) (1000000UL/frequency); + + motor_control.counter_update = true; } // Moves the motor to the home position (fully open). @@ -411,52 +425,42 @@ void motor_home() // Move the motor far enough in the OPEN direction (more than the total // possible excursion) in order to guarantee that the top limit switch will // be tripped. - motor_move(180, MOTOR_DIRECTION_OPEN); + motor_position_set(-90); + motor_speed_set(5000UL); } -// Moves the motor to the away position (fully closed). -void motor_away() { - // Move the motor far enough in the CLOSE direction (more than the total - // possible excursion) in order to guarantee that the bottom limit switch - // will be tripped. - motor_move(180, MOTOR_DIRECTION_CLOSE); -} - -//***************************************************************************** -// Interrupt Service Routines -//***************************************************************************** - -// Motor State Control ISR -static inline void motor_state_update() +static void motor_state_update() { - int32_t motor_step_delta = motor_control.step_command - motor_control.step_position; + int32_t motor_step_delta = motor_control.step_position_command - motor_control.step_position; if (motor_step_delta > MOTOR_CONTROL_STEP_ERROR) { if (digitalRead(PIN_LIMIT_BOTTOM) == PIN_LIMIT_TRIPPED) { - // Do not move the motor in the CLOSE direction if the bottom limit switch is tripped. - motor_state_set(MOTOR_STATE_HOLD); + // Do not move the motor in the CLOSE direction if the bottom limit + // switch is tripped. + motor_state_transition(MOTOR_STATE_HOLD); } else { - motor_state_set(MOTOR_STATE_CLOSE); + motor_state_transition(MOTOR_STATE_CLOSE); } } else if (motor_step_delta < -MOTOR_CONTROL_STEP_ERROR) { if (digitalRead(PIN_LIMIT_TOP) == PIN_LIMIT_TRIPPED) { - // Do not move the motor in the OPEN direction if the top limit switch is tripped. - motor_state_set(MOTOR_STATE_HOLD); - motor_position_zero(); + // Do not move the motor in the OPEN direction if the top limit switch is + // tripped. There is no need to go through motor_state_transition() + // because the zero reference point is set here. + motor_state_set_HOLD(); + motor_position_set_zero(); } else { - motor_state_set(MOTOR_STATE_OPEN); + motor_state_transition(MOTOR_STATE_OPEN); } } else { if ((motor_state == MOTOR_STATE_OPEN) || (motor_state == MOTOR_STATE_CLOSE)) { // Hold the motor in place in order to maintain the correct position // without having to zero the motor again. - motor_state_set(MOTOR_STATE_HOLD); + motor_state_transition(MOTOR_STATE_HOLD); } } } -// Motor Position Tracking ISR static inline void motor_position_update() { if (motor_state == MOTOR_STATE_OPEN) { @@ -470,19 +474,23 @@ static inline void motor_position_update() motor_control.microstep_position = 0; } } else { + static uint16_t error_count = 0; + DEBUG_PRINT("motor_position_update ERROR %d", ++error_count); // TODO: error } } +//***************************************************************************** +// Interrupt Service Routines +//***************************************************************************** + // Timer 3 BOTTOM ISR(TIMER3_OVF_vect) { - motor_state_update(); + // motor_state_update(); } // Timer 4 BOTTOM -// This routine is used for changing the motor from a moving state to a stopped -// state. ISR(TIMER4_OVF_vect) { motor_state_install_pending(); } @@ -490,19 +498,24 @@ ISR(TIMER4_OVF_vect) { // Timer 4 TOP ISR(TIMER4_COMPA_vect) { - motor_position_update(); + if (motor_control.counter_update) { + timer4_set_TOP(motor_control.counter_TOP); + motor_control.counter_update = false; + } - // TODO: Install pending motor PWM update command. + motor_position_update(); } //***************************************************************************** // motorHal Interface Implementation //***************************************************************************** -int motorHalInit(void) +int8_t motorHalInit(void) { ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { - timer3_setup(); + // TODO: Remove/relocate Timer 3 + // timer3_setup(); + timer4_setup(); // Motor Pin Configuration @@ -519,47 +532,42 @@ int motorHalInit(void) // avoided during WDT reset. // Move the motor to the home position to zero the motor position. - motor_home(); - while (motor_busy) {} + motorHalCommand(INT8_MIN, 5000U); + while(motorHalStatus() == HAL_IN_PROGRESS) {} delay(1000); // Move to the top of the bag. // TODO: This value will vary depending upon the installation location of the // top limit switch, as well as the type of bag. Ultimately, the sensors // should be used to find the top of the bag. - motor_move(15, MOTOR_DIRECTION_CLOSE); - while (motor_busy) {} + motorHalCommand(15, 5000U); + while (motorHalStatus() == HAL_IN_PROGRESS) {} delay(1000); return HAL_OK; } -// TODO: Implement duration. -int motorHalBegin(unsigned int direction, unsigned int distance, unsigned int duration) +// Absolute position, relative to the motor zero point. +// Speed given in MRPM (millirevolutions per second). +int8_t motorHalCommand(uint8_t position, uint16_t speed) { - motor_move_command.distance = distance; + motor_position_set(position); + motor_speed_set(speed); + + motor_state_update(); - if (direction == MOTOR_HAL_DIRECTION_INHALATION) { - motor_move_command.direction = MOTOR_DIRECTION_CLOSE; - } else if (direction == MOTOR_HAL_DIRECTION_EXHALATION) { - motor_move_command.direction = MOTOR_DIRECTION_OPEN; + if (motor_state_moving()) { + return HAL_IN_PROGRESS; } else { - return HAL_FAIL; + return HAL_OK; } - - motor_move_command.valid = true; - - return HAL_OK; } -int motorHalRun(void) +int8_t motorHalStatus(void) { - if (motor_move_command.valid) { - motor_move(motor_move_command.distance, motor_move_command.direction); - motor_move_command.valid = false; - } + motor_state_update(); - if (motor_busy) { + if (motor_state_moving()) { return HAL_IN_PROGRESS; } else { return HAL_OK; diff --git a/src/hal/motor.h b/src/hal/motor.h index 11eea3a..85cdcfc 100644 --- a/src/hal/motor.h +++ b/src/hal/motor.h @@ -1,19 +1,18 @@ - #ifndef __MOTOR_HAL_H__ #define __MOTOR_HAL_H__ #include "../hal/hal.h" -#define MOTOR_HAL_DIRECTION_INHALATION 0x55 -#define MOTOR_HAL_DIRECTION_EXHALATION 0xAF +#include // TODO: Doc -int motorHalInit(void); +int8_t motorHalInit(void); // TODO: Doc -int motorHalBegin(unsigned int direction, unsigned int distance, unsigned int duration); +int8_t motorHalCommand(uint8_t position, uint16_t speed); // TODO: Doc -int motorHalRun(void); +int8_t motorHalStatus(void); + +#endif /* __MOTOR_HAL_H__ */ -#endif /* __MOTOR_HAL_H__ */ \ No newline at end of file diff --git a/src/modules/control.cpp b/src/modules/control.cpp index 39f0b6e..79f6fa8 100644 --- a/src/modules/control.cpp +++ b/src/modules/control.cpp @@ -31,9 +31,6 @@ static struct pt controlThread; static struct timer breathTimer; static struct timer controlTimer; -static uint16_t totalMotorCompressionDistance; -static uint16_t totalMotorCompressionDuration; - static uint16_t targetVolume; static uint32_t totalBreathTime; static uint32_t targetInhalationTime; @@ -42,13 +39,11 @@ static uint32_t measuredInhalationTime; static uint32_t measuredExhalationTime; // Mid-level volume control function -static void updateVolumeControl(uint16_t* distance, uint16_t* duration) +static void updateVolumeControl(uint16_t* position, uint16_t* speed) { // TODO: fill-in this function; for now just perform a 40 degree motion - if (totalMotorCompressionDistance == 0) { - *distance = 40; - *duration = 0; - } + *position = 40; + *speed = 5000; } static PT_THREAD(controlThreadMain(struct pt* pt)) @@ -83,8 +78,6 @@ static PT_THREAD(controlThreadMain(struct pt* pt)) targetVolume = parameters.volumeRequested; // Initialize all - totalMotorCompressionDistance = 0; - totalMotorCompressionDuration = 0; measuredInhalationTime = 0; measuredExhalationTime = 0; @@ -97,31 +90,28 @@ static PT_THREAD(controlThreadMain(struct pt* pt)) } else if (control.state == CONTROL_INHALATION) { DEBUG_PRINT("state: CONTROL_INHALATION"); - uint16_t motorDistance = 0; - uint16_t motorDuration = 0; + uint16_t motorPosition = 0; + uint16_t motorSpeed = 0; // If in a volume controlled mode, run the Mid-level volume controller if ((parameters.ventilationMode == VENTILATOR_MODE_VC) || (parameters.ventilationMode == VENTILATOR_MODE_AC)) { // Mid-level controller function - updateVolumeControl(&motorDistance, &motorDuration); + updateVolumeControl(&motorPosition, &motorSpeed); } - // Update distance/duration accumulators (for exhalation) - totalMotorCompressionDistance += motorDistance; - totalMotorCompressionDuration += motorDuration; - // If the controller reports zero distance or the timer has expired, move - // on to next state, otherwi< Date: Thu, 23 Apr 2020 21:55:54 -0700 Subject: [PATCH 04/15] Add open-loop trajectory tracking control. --- src/modules/control.cpp | 208 ++++++++++++++++++++++++++++++++-------- 1 file changed, 167 insertions(+), 41 deletions(-) diff --git a/src/modules/control.cpp b/src/modules/control.cpp index 79f6fa8..7b9588f 100644 --- a/src/modules/control.cpp +++ b/src/modules/control.cpp @@ -14,13 +14,17 @@ #include "../modules/sensors.h" #include "../modules/parameters.h" -// #define DEBUG +#define DEBUG #define DEBUG_MODULE "control" #include "../util/debug.h" -#define INHALATION_OVERTIME 4 / 3 +#define CONTROL_LOOP_PERIOD (10 MSEC) + #define HOLD_TIME (200 MSEC) +#define INHALATION_OVERTIME(t) ((t) * 4 / 3) + + // Public Variables struct control control = { .state = CONTROL_IDLE @@ -38,12 +42,117 @@ static uint32_t targetInhalationTime; static uint32_t measuredInhalationTime; static uint32_t measuredExhalationTime; -// Mid-level volume control function -static void updateVolumeControl(uint16_t* position, uint16_t* speed) +// ***************** +// Control variables +// ***************** +// Position uint8_t +// Velocity millirev/min +// Velocity max = 20 rpm +static unsigned int currentPosition = 0; +static unsigned int targetPosition = 0; +static unsigned int targetVelocity = 0; +static unsigned int tolerancePosition = 0; +static unsigned int trapezoidRatio = 2; // TEMPORARILY SET +static uint32_t rampTime = 0; +static uint32_t platTime = 0; +static uint32_t elapsedTime = 0; +static float velocityScale = 0.0; +static float nomVelocity = 0.0; +static uint32_t breathTimerStateStart; + +// Pre-compute the trajectory based on known set points and timing requirements +static void computeTrajectory() +{ + // Debug + DEBUG_PRINT("COMPUTING TRAJECTORY"); + + // Update known measurements (position, volume estimation, etc.) + + // Calculate time sections of trajectory (CURRENTLY SAME FOR I/E PHASES DUE TO SENSOR DRIVER HOLD) + if (control.state == CONTROL_BEGIN_EXHALATION) { + + // Position information needs to be integrated to allow for adaptive positioning on exhalation + targetPosition = 5; + rampTime = (totalBreathTime - targetInhalationTime) / (2 + trapezoidRatio); + platTime = trapezoidRatio * rampTime; + nomVelocity = abs((float)targetPosition - (float)currentPosition) * 1000.0 * 1000000.0 * 60.0 / ((float)(rampTime + platTime) * 360.0); + + } else if (control.state == CONTROL_BEGIN_INHALATION) { + + // EVENTUALLY REFACTORING FOR VOLUME TRAJECTORY, temporarily position tracking for testing + targetPosition = 40; + rampTime = targetInhalationTime / (2 + trapezoidRatio); + platTime = trapezoidRatio * rampTime; + nomVelocity = abs((float)targetPosition - (float)currentPosition) * 1000.0 * 1000000.0 * 60.0 / ((float)(rampTime + platTime) * 360.0); + + } else { + // Blank + } +} + +// Track timing and scale output based on location in trajectory +static void updateTrajectory() { - // TODO: fill-in this function; for now just perform a 40 degree motion - *position = 40; - *speed = 5000; + + // Determine elapsed time and place along trajectory + elapsedTime = timerHalCurrent(&breathTimer) - breathTimerStateStart; + + if (elapsedTime < rampTime) { + // Ramp up + velocityScale = ((float)elapsedTime / (float)rampTime); + targetVelocity = (unsigned int)(nomVelocity * velocityScale); + + } else if ((elapsedTime > rampTime) && (elapsedTime < (platTime + rampTime))) { + // Plateau + velocityScale = 1.0; + targetVelocity = (unsigned int)(nomVelocity * velocityScale); + + } else { + // Ramp down + velocityScale = (float)(2 * rampTime + platTime - elapsedTime) / (float)rampTime; + targetVelocity = (unsigned int)(nomVelocity * velocityScale); + + } + + // Debug Output Section + DEBUG_PRINT_EVERY(100, "Target Position: %lu ; Nominal Velocity: %lu ; Target Velocity: %lu", + (uint32_t)targetPosition, (uint32_t)nomVelocity, (uint32_t)targetVelocity); + // DEBUG_PRINT_EVERY(1000, "Elapsed Time: %lu ; Ramp Time: %lu ; Velocity Scale: %lu", elapsedTime, rampTime, (uint32_t)(1000.0 * velocityScale)); + // DEBUG_PRINT_EVERY(1000, "Proportion of Phase: %lu", velocityScale); + +} + +// Update control commands and handle ISR/control flags +static int updateControl() +{ + + // Update trajectory to get instantaneous target air flow + updateTrajectory(); + + // ************************************************************** + // Here is where closed loop control structure will be developed + // ************************************************************** + // Depending on whether we are in inhalation or exhalation, we will + // switch between closed loop air flow tracking and closed loop position + // tracking. The updateControl() function will accept a flag set by an ISR. + + // Send motor commands + motorHalCommand(targetPosition, targetVelocity); + + // Return unfinished + return 0; + +} + +static bool checkInhalationTimeout(void) +{ + return ((timerHalCurrent(&breathTimer) - breathTimerStateStart) >= + INHALATION_OVERTIME(targetInhalationTime)); +} + +static bool checkExhalationTimeout(void) +{ + return (timerHalRun(&breathTimer) != HAL_IN_PROGRESS); } static PT_THREAD(controlThreadMain(struct pt* pt)) @@ -81,37 +190,41 @@ static PT_THREAD(controlThreadMain(struct pt* pt)) measuredInhalationTime = 0; measuredExhalationTime = 0; - // Kickoff timers for monitoring breath/stages of breathing + // Kickoff timer for monitoring breath of breathing timerHalBegin(&breathTimer, totalBreathTime); - timerHalBegin(&controlTimer, targetInhalationTime * INHALATION_OVERTIME); + breathTimerStateStart = timerHalCurrent(&breathTimer); + + // Compute trajectory + computeTrajectory(); + control.state = CONTROL_INHALATION; } else if (control.state == CONTROL_INHALATION) { DEBUG_PRINT("state: CONTROL_INHALATION"); - uint16_t motorPosition = 0; - uint16_t motorSpeed = 0; - - // If in a volume controlled mode, run the Mid-level volume controller - if ((parameters.ventilationMode == VENTILATOR_MODE_VC) || - (parameters.ventilationMode == VENTILATOR_MODE_AC)) { - - // Mid-level controller function - updateVolumeControl(&motorPosition, &motorSpeed); + // Kick off control loop timer, ensuring the control loop runs periodically + timerHalBegin(&controlTimer, CONTROL_LOOP_PERIOD); + + // TODO: Different modes? + updateControl(); + + // Wait until either the control loop timer has expired or until the timeout + // condition has been reached; assuming the velocity isnt 0 + if (targetVelocity == 0) { + PT_WAIT_UNTIL(pt, ((timerHalRun(&controlTimer) != HAL_IN_PROGRESS) || + checkInhalationTimeout())); } - // If the controller reports zero distance or the timer has expired, move - // on to next state, otherwise run the motor as specified by the controller - // TODO: Fix up conditions based on new motor hal api - if (timerHalRun(&controlTimer) == HAL_TIMEOUT) { + // If either a timeout occurred or the controller is setting the velocity + // to 0, we have arrived at the maximum compression point, move on to the + // next part of the breath + if (checkInhalationTimeout() || (targetVelocity == 0)) { + // Update some things on state exit + currentPosition = targetPosition; measuredInhalationTime = timerHalCurrent(&breathTimer); + control.state = CONTROL_BEGIN_HOLD_IN; - } else { - motorHalCommand(motorPosition, motorSpeed); // TODO: Fix the speed - // TODO: Wait until motor reaches destination? Or try to parallelize - // parameters re-issue motor control setpoints before completing last ones? - PT_WAIT_UNTIL(pt, motorHalStatus() != HAL_IN_PROGRESS); } } else if (control.state == CONTROL_BEGIN_HOLD_IN) { @@ -131,25 +244,37 @@ static PT_THREAD(controlThreadMain(struct pt* pt)) } else if (control.state == CONTROL_BEGIN_EXHALATION) { DEBUG_PRINT("state: CONTROL_BEGIN_EXHALATION"); - // Since patient exhalation is not dependent on the bag, allow the bag to - // decompress back with fixed parameters for now - // TODO: consider if patient takes breathe before motor has completely moved back - motorHalCommand(5, 5000U); + // Compute trajectory + computeTrajectory(); + + breathTimerStateStart = timerHalCurrent(&breathTimer); + control.state = CONTROL_EXHALATION; } else if (control.state == CONTROL_EXHALATION) { DEBUG_PRINT("state: CONTROL_EXHALATION"); - // Wait for the motor to move back - // TODO: consider if patient takes breathe before motor has completely moved back? - // probably cant start the breath until the motor moves back anyway, right? - PT_WAIT_UNTIL(pt, ((motorHalStatus() != HAL_IN_PROGRESS) || - (timerHalRun(&breathTimer) != HAL_IN_PROGRESS))); - - // Now wait for the current exhalation time (now the remainder of the breath - // timer) to expire or for a breath to be detected if in an assisted mode + + // Kick off control loop timer, ensuring the control loop runs periodically + timerHalBegin(&controlTimer, CONTROL_LOOP_PERIOD); + + // TODO: Different modes? + updateControl(); + + // Wait until either the control loop timer has expired or until the timeout + // condition has been reached; assuming the velocity isnt 0 + // TODO: Consider what a timeout here means, it means the motor wasn't + // able to get all the way back, should we really keep going? + if (targetVelocity == 0) { + PT_WAIT_UNTIL(pt, ((timerHalRun(&controlTimer) != HAL_IN_PROGRESS) || + checkExhalationTimeout())); + } + + currentPosition = targetPosition; + + // 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) || - ((parameters.ventilationMode == VENTILATOR_MODE_AC) && - sensors.inhalationDetected))); + (sensors.inhalationDetected))); // Calculate and update the measured times uint32_t currentBreathTime = timerHalCurrent(&breathTimer); @@ -168,6 +293,7 @@ static PT_THREAD(controlThreadMain(struct pt* pt)) control.state = CONTROL_IDLE; } + // TODO: Consider removing this yield to improve control loop timing PT_YIELD(pt); } From e91d7f5ee5dad069895e28dce94bda2a1ea18a58 Mon Sep 17 00:00:00 2001 From: JanS Date: Fri, 24 Apr 2020 19:12:30 +0200 Subject: [PATCH 05/15] First implementation of closed loop PID control not tested in HW yet --- src/modules/control.cpp | 191 ++++++++++++++++++++++++++++------------ 1 file changed, 134 insertions(+), 57 deletions(-) diff --git a/src/modules/control.cpp b/src/modules/control.cpp index 7b9588f..0d2250c 100644 --- a/src/modules/control.cpp +++ b/src/modules/control.cpp @@ -24,6 +24,8 @@ #define INHALATION_OVERTIME(t) ((t) * 4 / 3) +#define MIN_VELOCITY 2 // TODO: Fix this + // Public Variables struct control control = { @@ -42,6 +44,8 @@ static uint32_t targetInhalationTime; static uint32_t measuredInhalationTime; static uint32_t measuredExhalationTime; +static bool controlComplete; + // ***************** // Control variables // ***************** @@ -50,15 +54,15 @@ static uint32_t measuredExhalationTime; // Velocity max = 20 rpm static unsigned int currentPosition = 0; static unsigned int targetPosition = 0; -static unsigned int targetVelocity = 0; +static float targetAirFlow = 0.0f; static unsigned int tolerancePosition = 0; static unsigned int trapezoidRatio = 2; // TEMPORARILY SET static uint32_t rampTime = 0; static uint32_t platTime = 0; static uint32_t elapsedTime = 0; -static float velocityScale = 0.0; -static float nomVelocity = 0.0; -static uint32_t breathTimerStateStart; +static float velocityScale = 0.0f; +static float nomVelocity = 0.0f; +static uint32_t breathTimerStateStart = 0; // Pre-compute the trajectory based on known set points and timing requirements static void computeTrajectory() @@ -73,17 +77,18 @@ static void computeTrajectory() // Position information needs to be integrated to allow for adaptive positioning on exhalation targetPosition = 5; - rampTime = (totalBreathTime - targetInhalationTime) / (2 + trapezoidRatio); + rampTime = (totalBreathTime - targetInhalationTime) / (2.0f + trapezoidRatio); platTime = trapezoidRatio * rampTime; - nomVelocity = abs((float)targetPosition - (float)currentPosition) * 1000.0 * 1000000.0 * 60.0 / ((float)(rampTime + platTime) * 360.0); + nomVelocity = abs((float)targetPosition - (float)currentPosition) * 1000.0f * 1000000.0f * 60.0f / ((float)(rampTime + platTime) * 360.0f); + } else if (control.state == CONTROL_BEGIN_INHALATION) { // EVENTUALLY REFACTORING FOR VOLUME TRAJECTORY, temporarily position tracking for testing targetPosition = 40; - rampTime = targetInhalationTime / (2 + trapezoidRatio); + rampTime = targetInhalationTime / (2.0f + trapezoidRatio); platTime = trapezoidRatio * rampTime; - nomVelocity = abs((float)targetPosition - (float)currentPosition) * 1000.0 * 1000000.0 * 60.0 / ((float)(rampTime + platTime) * 360.0); + nomVelocity = abs((float)targetPosition - (float)currentPosition) * 1000.0f * 1000000.0f * 60.0f / ((float)(rampTime + platTime) * 360.0f); } else { // Blank @@ -100,48 +105,115 @@ static void updateTrajectory() if (elapsedTime < rampTime) { // Ramp up velocityScale = ((float)elapsedTime / (float)rampTime); - targetVelocity = (unsigned int)(nomVelocity * velocityScale); } else if ((elapsedTime > rampTime) && (elapsedTime < (platTime + rampTime))) { // Plateau velocityScale = 1.0; - targetVelocity = (unsigned int)(nomVelocity * velocityScale); } else { // Ramp down velocityScale = (float)(2 * rampTime + platTime - elapsedTime) / (float)rampTime; - targetVelocity = (unsigned int)(nomVelocity * velocityScale); } - // Debug Output Section + targetAirFlow = (nomVelocity * velocityScale); + // Debug Output Section DEBUG_PRINT_EVERY(100, "Target Position: %lu ; Nominal Velocity: %lu ; Target Velocity: %lu", - (uint32_t)targetPosition, (uint32_t)nomVelocity, (uint32_t)targetVelocity); + (uint32_t)targetPosition, (uint32_t)nomVelocity, (uint32_t)targetAirFlow); // DEBUG_PRINT_EVERY(1000, "Elapsed Time: %lu ; Ramp Time: %lu ; Velocity Scale: %lu", elapsedTime, rampTime, (uint32_t)(1000.0 * velocityScale)); // DEBUG_PRINT_EVERY(1000, "Proportion of Phase: %lu", velocityScale); } // Update control commands and handle ISR/control flags -static int updateControl() +static int updateControl(void) { - // Update trajectory to get instantaneous target air flow - updateTrajectory(); + float flowSensorInput; + static float lastFlowSensorInput=0.0f; + float controlP,controlD; + static float controlI=0.0f; + float controlOut; + float controlOutLimited; + + //TEMPORARY ASSIGNMENT. SHOULD BE STORED IN PARAMETERS + float Kf=0.8f; + float Kp=0.1f; + float Ki=.0f; + float Kd=.0f; + float KiMax=10.0f; //max speed adjustment because of I-part in % of nominalVelocity + float controlMax=20.0f; //max speed adjustment of current target velocity + + //Timing + // TODO: Use metrics here instead + long startMicros=0; + long timeToComputeControl=0; + static long timeToComputeControlFiltered=0; + //startMicros=micros(); + + // Update trajectory to get instantaneous target air flow + updateTrajectory(); + + // ************************************************************** + // Here is where closed loop control structure will be developed + // ************************************************************** + // Depending on whether we are in inhalation or exhalation, we will + // switch between closed loop air flow tracking and closed loop position + // tracking. The updateControl() function will accept a flag set by an ISR. + + + //read sensor inputs + flowSensorInput=targetAirFlow;//needs to be getFlowSensorInput + + //calculate proportional part + controlP=targetAirFlow-flowSensorInput; + + //calculate differential part + controlD=lastFlowSensorInput-flowSensorInput; + + //calculate integral part + controlI+=controlP; + + //limit integral part + if ((Ki*controlI)>(nomVelocity*KiMax/100.0f)) + controlI=(nomVelocity*KiMax/100.0f)/Ki; - // ************************************************************** - // Here is where closed loop control structure will be developed - // ************************************************************** - // Depending on whether we are in inhalation or exhalation, we will - // switch between closed loop air flow tracking and closed loop position - // tracking. The updateControl() function will accept a flag set by an ISR. + if ((Ki*controlI)<(-nomVelocity*KiMax/100.0f)) + controlI=-(nomVelocity*KiMax/100.0f)/Ki; - // Send motor commands - motorHalCommand(targetPosition, targetVelocity); + //limit and output - // Return unfinished - return 0; + controlOut=Kp*controlP+Kd*controlD+Ki*controlI; + + if (controlOut>(controlMax/100.0f*targetAirFlow)) + controlOut=(controlMax/100.0f*targetAirFlow); + + if (controlOut<-(controlMax/100.0f*targetAirFlow)) + controlOut=-(controlMax/100.0f*targetAirFlow); + + //final control output is feed forward + limited controlOut + controlOutLimited=Kf*targetAirFlow+controlOut; + + // Send motor commands + motorHalCommand(targetPosition, controlOutLimited); + + lastFlowSensorInput=flowSensorInput; + + //timeToComputeControl=micros()-startMicros; + + if (timeToComputeControl>0) //micros() overruns every 70mins, catch this here + { + timeToComputeControlFiltered=9*timeToComputeControlFiltered+timeToComputeControl; + timeToComputeControlFiltered/=10; + DEBUG_PRINT_EVERY(1000,"Control Stats: Avg us: %ul\n",timeToComputeControlFiltered); + } + // Return finished + if (controlOutLimited < MIN_VELOCITY) { + return 1; + } + // Return unfinished + return 0; } static bool checkInhalationTimeout(void) @@ -194,6 +266,7 @@ static PT_THREAD(controlThreadMain(struct pt* pt)) timerHalBegin(&breathTimer, totalBreathTime); breathTimerStateStart = timerHalCurrent(&breathTimer); + controlComplete = false; // Compute trajectory computeTrajectory(); @@ -203,29 +276,28 @@ static PT_THREAD(controlThreadMain(struct pt* pt)) } else if (control.state == CONTROL_INHALATION) { DEBUG_PRINT("state: CONTROL_INHALATION"); - // Kick off control loop timer, ensuring the control loop runs periodically - timerHalBegin(&controlTimer, CONTROL_LOOP_PERIOD); + while (1) { + // Kick off control loop timer, ensuring the control loop runs periodically + timerHalBegin(&controlTimer, CONTROL_LOOP_PERIOD); - // TODO: Different modes? - updateControl(); + // TODO: Different modes? + controlComplete = updateControl(); - // Wait until either the control loop timer has expired or until the timeout - // condition has been reached; assuming the velocity isnt 0 - if (targetVelocity == 0) { - PT_WAIT_UNTIL(pt, ((timerHalRun(&controlTimer) != HAL_IN_PROGRESS) || - checkInhalationTimeout())); + // 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 exit this state + if (!controlComplete && !checkInhalationTimeout()) { + PT_WAIT_UNTIL(pt, timerHalRun(&controlTimer) != HAL_IN_PROGRESS); + } else { + break; + } } - - // If either a timeout occurred or the controller is setting the velocity - // to 0, we have arrived at the maximum compression point, move on to the - // next part of the breath - if (checkInhalationTimeout() || (targetVelocity == 0)) { - // Update some things on state exit - currentPosition = targetPosition; - measuredInhalationTime = timerHalCurrent(&breathTimer); + + // Update some things on state exit + currentPosition = targetPosition; + measuredInhalationTime = timerHalCurrent(&breathTimer); - control.state = CONTROL_BEGIN_HOLD_IN; - } + control.state = CONTROL_BEGIN_HOLD_IN; } else if (control.state == CONTROL_BEGIN_HOLD_IN) { DEBUG_PRINT("state: CONTROL_BEGIN_HOLD_IN"); @@ -248,27 +320,33 @@ static PT_THREAD(controlThreadMain(struct pt* pt)) computeTrajectory(); breathTimerStateStart = timerHalCurrent(&breathTimer); + controlComplete = false; control.state = CONTROL_EXHALATION; } else if (control.state == CONTROL_EXHALATION) { DEBUG_PRINT("state: CONTROL_EXHALATION"); - // Kick off control loop timer, ensuring the control loop runs periodically - timerHalBegin(&controlTimer, CONTROL_LOOP_PERIOD); + while (1) { + // Kick off control loop timer, ensuring the control loop runs periodically + timerHalBegin(&controlTimer, CONTROL_LOOP_PERIOD); - // TODO: Different modes? - updateControl(); + // TODO: Different modes? + controlComplete = updateControl(); - // Wait until either the control loop timer has expired or until the timeout - // condition has been reached; assuming the velocity isnt 0 - // TODO: Consider what a timeout here means, it means the motor wasn't - // able to get all the way back, should we really keep going? - if (targetVelocity == 0) { - PT_WAIT_UNTIL(pt, ((timerHalRun(&controlTimer) != HAL_IN_PROGRESS) || - checkExhalationTimeout())); + // 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 + // TODO: Consider what a timeout here means, it means the motor wasn't + // able to get all the way back, should we really keep going? + if (!controlComplete && !checkInhalationTimeout()) { + PT_WAIT_UNTIL(pt, timerHalRun(&controlTimer) != HAL_IN_PROGRESS); + } else { + break; + } } + // TODO: look into this currentPosition = targetPosition; // At this point, either wait for the breath timer to expire or find a new @@ -293,7 +371,6 @@ static PT_THREAD(controlThreadMain(struct pt* pt)) control.state = CONTROL_IDLE; } - // TODO: Consider removing this yield to improve control loop timing PT_YIELD(pt); } From 65ede1f96652104ddfdeaa443b9bf01099f8d83a Mon Sep 17 00:00:00 2001 From: "Rosen, Michael R" Date: Tue, 28 Apr 2020 17:56:13 -0700 Subject: [PATCH 06/15] control: use metrics to time control loop Signed-off-by: Rosen, Michael R --- src/modules/control.cpp | 25 +++++++++++-------------- 1 file changed, 11 insertions(+), 14 deletions(-) diff --git a/src/modules/control.cpp b/src/modules/control.cpp index 0d2250c..fd2cde8 100644 --- a/src/modules/control.cpp +++ b/src/modules/control.cpp @@ -14,6 +14,8 @@ #include "../modules/sensors.h" #include "../modules/parameters.h" +#include "../util/metrics.h" + #define DEBUG #define DEBUG_MODULE "control" #include "../util/debug.h" @@ -64,6 +66,8 @@ static float velocityScale = 0.0f; static float nomVelocity = 0.0f; static uint32_t breathTimerStateStart = 0; +static struct metrics midControlTiming; + // Pre-compute the trajectory based on known set points and timing requirements static void computeTrajectory() { @@ -143,13 +147,8 @@ static int updateControl(void) float Kd=.0f; float KiMax=10.0f; //max speed adjustment because of I-part in % of nominalVelocity float controlMax=20.0f; //max speed adjustment of current target velocity - - //Timing - // TODO: Use metrics here instead - long startMicros=0; - long timeToComputeControl=0; - static long timeToComputeControlFiltered=0; - //startMicros=micros(); + + metricsStart(&midControlTiming); // Update trajectory to get instantaneous target air flow updateTrajectory(); @@ -200,14 +199,10 @@ static int updateControl(void) lastFlowSensorInput=flowSensorInput; - //timeToComputeControl=micros()-startMicros; + metricsStop(&midControlTiming); - if (timeToComputeControl>0) //micros() overruns every 70mins, catch this here - { - timeToComputeControlFiltered=9*timeToComputeControlFiltered+timeToComputeControl; - timeToComputeControlFiltered/=10; - DEBUG_PRINT_EVERY(1000,"Control Stats: Avg us: %ul\n",timeToComputeControlFiltered); - } + DEBUG_PRINT_EVERY(1000,"Control Stats: Avg us: %ul\n",midControlTiming.average); + // Return finished if (controlOutLimited < MIN_VELOCITY) { return 1; @@ -231,6 +226,8 @@ static PT_THREAD(controlThreadMain(struct pt* pt)) { PT_BEGIN(pt); + metricsReset(&midControlTiming); + // Current Settings // 15 BPM // I:E 1:2 From 544e142f3fdfbb56a0dc4fa27142792d5ebf3d43 Mon Sep 17 00:00:00 2001 From: "Rosen, Michael R" Date: Tue, 28 Apr 2020 18:24:13 -0700 Subject: [PATCH 07/15] control: temporarilly fix requested IE ratio Currently the serial control sends the IE as an enumeration when it should be a fixed point number; for now fix the value to 1:1.5 Signed-off-by: Rosen, Michael R --- src/modules/control.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/modules/control.cpp b/src/modules/control.cpp index fd2cde8..233be21 100644 --- a/src/modules/control.cpp +++ b/src/modules/control.cpp @@ -205,6 +205,7 @@ static int updateControl(void) // Return finished if (controlOutLimited < MIN_VELOCITY) { + DEBUG_PRINT("Target Reached"); return 1; } // Return unfinished @@ -250,6 +251,10 @@ static PT_THREAD(controlThreadMain(struct pt* pt)) } else if (control.state == CONTROL_BEGIN_INHALATION) { DEBUG_PRINT("state: CONTROL_BEGIN_INHALATION"); + // TODO: Actually sync up with how ie is supposed to be represented + // for next, fix IE at 1:1.5 (1 / 2.5 = 0x0066) + parameters.ieRatioRequested = 0x0066; + // Collect all set points from parameters totalBreathTime = (60 SEC) / parameters.respirationRateRequested; targetInhalationTime = (parameters.ieRatioRequested * totalBreathTime) >> 8; From 014c21e6c1b8d82f13ab9071cf8d11e332f55bfc Mon Sep 17 00:00:00 2001 From: "Rosen, Michael R" Date: Wed, 29 Apr 2020 10:47:08 -0700 Subject: [PATCH 08/15] control: fix a number of issues from Jan . Fix minor issues in stop mid control loop . Refactor timers to be periodic to improve precision of timing loops Signed-off-by: Rosen, Michael R --- src/hal/timer.cpp | 10 +++++- src/hal/timer.h | 4 ++- src/modules/control.cpp | 69 +++++++++++++++++++++++++---------------- src/modules/sensors.cpp | 6 ++-- 4 files changed, 58 insertions(+), 31 deletions(-) diff --git a/src/hal/timer.cpp b/src/hal/timer.cpp index 524bc55..7edf796 100644 --- a/src/hal/timer.cpp +++ b/src/hal/timer.cpp @@ -1,5 +1,6 @@ #include +#include #include "../hal/hal.h" #include "../hal/timer.h" @@ -12,11 +13,12 @@ int timerHalInit(void) return HAL_OK; } -int timerHalBegin(struct timer* timer, uint32_t duration) +int timerHalBegin(struct timer* timer, uint32_t duration, bool periodic) { if (timer) { timer->start = (uint32_t) micros(); timer->duration = duration; + timer->periodic = periodic; return HAL_OK; } return HAL_FAIL; @@ -25,9 +27,15 @@ int timerHalBegin(struct timer* timer, uint32_t duration) int timerHalRun(struct timer* timer) { if (timer) { + // Check to see if the time has expired if (((uint32_t) micros()) - timer->start < timer->duration) { return HAL_IN_PROGRESS; } else { + // In order to keep more precise periodic timers, add the duration + // to the start time once the timer has timed out + if (timer->periodic) { + timer->start += timer->duration; + } return HAL_TIMEOUT; } } diff --git a/src/hal/timer.h b/src/hal/timer.h index 16a6085..ceec626 100644 --- a/src/hal/timer.h +++ b/src/hal/timer.h @@ -3,6 +3,7 @@ #define __TIMER_HAL_H__ #include +#include #include "../hal/hal.h" @@ -14,13 +15,14 @@ struct timer { uint32_t start; uint32_t duration; + bool periodic; }; // TODO: Doc int timerHalInit(void); // TODO: Doc -int timerHalBegin(struct timer* timer, uint32_t duration); +int timerHalBegin(struct timer* timer, uint32_t duration, bool periodic); // TODO: Doc int timerHalRun(struct timer* timer); diff --git a/src/modules/control.cpp b/src/modules/control.cpp index 233be21..688c55a 100644 --- a/src/modules/control.cpp +++ b/src/modules/control.cpp @@ -16,7 +16,7 @@ #include "../util/metrics.h" -#define DEBUG +//#define DEBUG #define DEBUG_MODULE "control" #include "../util/debug.h" @@ -100,7 +100,7 @@ static void computeTrajectory() } // Track timing and scale output based on location in trajectory -static void updateTrajectory() +static int updateTrajectory() { // Determine elapsed time and place along trajectory @@ -114,19 +114,23 @@ static void updateTrajectory() // Plateau velocityScale = 1.0; - } else { + } else if (elapsedTime<(platTime + 2*rampTime)) { // Ramp down velocityScale = (float)(2 * rampTime + platTime - elapsedTime) / (float)rampTime; - + }else + { + velocityScale=0.0f; + targetAirFlow=0.0f; + return 1; } targetAirFlow = (nomVelocity * velocityScale); // Debug Output Section - DEBUG_PRINT_EVERY(100, "Target Position: %lu ; Nominal Velocity: %lu ; Target Velocity: %lu", + DEBUG_PRINT_EVERY(10, "Target Position: %lu ; Nominal Velocity: %lu ; Target Velocity: %lu", (uint32_t)targetPosition, (uint32_t)nomVelocity, (uint32_t)targetAirFlow); // DEBUG_PRINT_EVERY(1000, "Elapsed Time: %lu ; Ramp Time: %lu ; Velocity Scale: %lu", elapsedTime, rampTime, (uint32_t)(1000.0 * velocityScale)); // DEBUG_PRINT_EVERY(1000, "Proportion of Phase: %lu", velocityScale); - + return 0; } // Update control commands and handle ISR/control flags @@ -144,14 +148,29 @@ static int updateControl(void) float Kf=0.8f; float Kp=0.1f; float Ki=.0f; - float Kd=.0f; + float Kd=0.1f; float KiMax=10.0f; //max speed adjustment because of I-part in % of nominalVelocity float controlMax=20.0f; //max speed adjustment of current target velocity + + if (control.state == CONTROL_EXHALATION) + { + //deactivate controller for exhilation + Kf=1.0f; + Kp=.0f; + Ki=.0f; + Kd=.0f; + } + metricsStart(&midControlTiming); // Update trajectory to get instantaneous target air flow - updateTrajectory(); + if (updateTrajectory()>0) + { + motorHalCommand(targetPosition,0); + DEBUG_PRINT("Target Reached"); + return 1; + } // ************************************************************** // Here is where closed loop control structure will be developed @@ -162,7 +181,7 @@ static int updateControl(void) //read sensor inputs - flowSensorInput=targetAirFlow;//needs to be getFlowSensorInput + flowSensorInput=sensors.currentFlow;//needs to be getFlowSensorInput //calculate proportional part controlP=targetAirFlow-flowSensorInput; @@ -201,13 +220,8 @@ static int updateControl(void) metricsStop(&midControlTiming); - DEBUG_PRINT_EVERY(1000,"Control Stats: Avg us: %ul\n",midControlTiming.average); + DEBUG_PRINT_EVERY(100,"Control Stats: Avg us: %u\n",midControlTiming.average); - // Return finished - if (controlOutLimited < MIN_VELOCITY) { - DEBUG_PRINT("Target Reached"); - return 1; - } // Return unfinished return 0; } @@ -254,10 +268,11 @@ static PT_THREAD(controlThreadMain(struct pt* pt)) // TODO: Actually sync up with how ie is supposed to be represented // for next, fix IE at 1:1.5 (1 / 2.5 = 0x0066) parameters.ieRatioRequested = 0x0066; - + parameters.respirationRateRequested = 15; + // Collect all set points from parameters totalBreathTime = (60 SEC) / parameters.respirationRateRequested; - targetInhalationTime = (parameters.ieRatioRequested * totalBreathTime) >> 8; + targetInhalationTime = (parameters.ieRatioRequested * totalBreathTime) >> 8; // TODO: address fixed point math targetVolume = parameters.volumeRequested; // Initialize all @@ -265,7 +280,7 @@ static PT_THREAD(controlThreadMain(struct pt* pt)) measuredExhalationTime = 0; // Kickoff timer for monitoring breath of breathing - timerHalBegin(&breathTimer, totalBreathTime); + timerHalBegin(&breathTimer, totalBreathTime, false); breathTimerStateStart = timerHalCurrent(&breathTimer); controlComplete = false; @@ -277,11 +292,11 @@ static PT_THREAD(controlThreadMain(struct pt* pt)) } else if (control.state == CONTROL_INHALATION) { DEBUG_PRINT("state: CONTROL_INHALATION"); + + // Begin control loop timer when control starts + timerHalBegin(&controlTimer, CONTROL_LOOP_PERIOD, true); while (1) { - // Kick off control loop timer, ensuring the control loop runs periodically - timerHalBegin(&controlTimer, CONTROL_LOOP_PERIOD); - // TODO: Different modes? controlComplete = updateControl(); @@ -305,7 +320,7 @@ static PT_THREAD(controlThreadMain(struct pt* pt)) DEBUG_PRINT("state: CONTROL_BEGIN_HOLD_IN"); // Setup the hold timer - timerHalBegin(&controlTimer, HOLD_TIME); + timerHalBegin(&controlTimer, HOLD_TIME, false); control.state = CONTROL_HOLD_IN; @@ -325,14 +340,14 @@ static PT_THREAD(controlThreadMain(struct pt* pt)) controlComplete = false; control.state = CONTROL_EXHALATION; - + } else if (control.state == CONTROL_EXHALATION) { DEBUG_PRINT("state: CONTROL_EXHALATION"); - while (1) { - // Kick off control loop timer, ensuring the control loop runs periodically - timerHalBegin(&controlTimer, CONTROL_LOOP_PERIOD); + // Begin control loop timer when control starts + timerHalBegin(&controlTimer, CONTROL_LOOP_PERIOD, true); + while (1) { // TODO: Different modes? controlComplete = updateControl(); @@ -359,7 +374,7 @@ static PT_THREAD(controlThreadMain(struct pt* pt)) // Calculate and update the measured times uint32_t currentBreathTime = timerHalCurrent(&breathTimer); measuredExhalationTime = currentBreathTime - HOLD_TIME - measuredInhalationTime; - control.ieRatioMeasured = (measuredExhalationTime << 8) / measuredInhalationTime; + control.ieRatioMeasured = (measuredExhalationTime << 8) / measuredInhalationTime; // TODO: address fixed point math control.respirationRateMeasured = (60 SEC) / (currentBreathTime USEC); control.breathCount++; diff --git a/src/modules/sensors.cpp b/src/modules/sensors.cpp index c2d7884..12e83eb 100644 --- a/src/modules/sensors.cpp +++ b/src/modules/sensors.cpp @@ -68,7 +68,8 @@ static PT_THREAD(sensorsPressureThreadMain(struct pt* pt)) PT_BEGIN(pt); // Kick off sampling timer - timerHalBegin(&pressureTimer, PRESSURE_SAMPLING_PERIOD); + // TODO: refactor to use periodic timer + timerHalBegin(&pressureTimer, PRESSURE_SAMPLING_PERIOD, false); int16_t pressure; pressureSensorHalGetValue(&pressure); // get pressure, in [0.1mmH2O] @@ -166,7 +167,8 @@ static PT_THREAD(sensorsAirFlowThreadMain(struct pt* pt)) PT_BEGIN(pt); // Kick off sampling timer - timerHalBegin(&airflowTimer, AIRFLOW_SAMPLING_PERIOD); + // TODO: refactor to use periodic timer + timerHalBegin(&airflowTimer, AIRFLOW_SAMPLING_PERIOD, false); int16_t airflow; int16_t airvolume; From a140852bf71fabe45f3f4917b2db4a0530281219 Mon Sep 17 00:00:00 2001 From: "Rosen, Michael R" Date: Wed, 29 Apr 2020 18:58:14 -0700 Subject: [PATCH 09/15] sensors: refactor sensors hal and module significantly Using Jan's work as a base, refactor sensor hal to sample ADC without timer. Refactor alot of the sensor module to incorporate the refactored sensors hal and timer hal. Signed-off-by: Rosen, Michael R --- src/hal/sensor/adc.h | 49 ++++++ src/hal/sensor/airflow.cpp | 46 ++++++ src/hal/sensor/airflow.h | 18 +++ src/hal/sensor/pressure.cpp | 41 +++++ src/hal/sensor/pressure.h | 18 +++ src/hal/sensor/sensor.cpp | 127 --------------- src/hal/sensor/sensor.h | 24 --- src/modules/sensors.cpp | 308 +++++++++++++++++++++--------------- 8 files changed, 356 insertions(+), 275 deletions(-) create mode 100644 src/hal/sensor/adc.h create mode 100644 src/hal/sensor/airflow.cpp create mode 100644 src/hal/sensor/airflow.h create mode 100644 src/hal/sensor/pressure.cpp create mode 100644 src/hal/sensor/pressure.h delete mode 100644 src/hal/sensor/sensor.cpp delete mode 100644 src/hal/sensor/sensor.h diff --git a/src/hal/sensor/adc.h b/src/hal/sensor/adc.h new file mode 100644 index 0000000..24a8584 --- /dev/null +++ b/src/hal/sensor/adc.h @@ -0,0 +1,49 @@ + +#ifndef __ADC_SENSOR_HAL_H__ +#define __ADC_SENSOR_HAL_H__ + +#include + +#include + +// As multiple sensors use the ADC, add several macros to +// define common functions for the ADC with writing a separate +// driver for it + +// Check if ADC is enabled/initialized +#define adcHalEnabled() (bit_is_set(ADCSRA, ADEN)) + +// Check if ADC is currently doing a conversion or has data ready that hasnt been claimed +#define adcHalBusy() (bit_is_set(ADCSRA, ADSC) || bit_is_set(ADCSRA, ADIF)) + +// Check if ADC is currently doing a conversion, so data can be retreived once complete +#define adcHalInProgress() (bit_is_set(ADCSRA, ADSC)) + +// Initialized ADC to use AVcc Reference +#define adcHalInit() \ + do { \ + ADMUX = bit(REFS0); \ + ADCSRA = bit(ADEN) | bit(ADPS0) | bit(ADPS1) | bit(ADPS2); \ + } while (0); + +// Begin an ADC conversion; first set the correct mux bits, then set the start bit +#define adcHalBegin(pin) \ + do { \ + ADMUX = (ADMUX & (~0x7)) | (pin & 0x7); \ + ADCSRB = (ADCSRB & (~bit(MUX5))) | ((pin > 7) ? bit(MUX5) : 0); \ + ADCSRA |= bit(ADSC); \ + } while (0); + +// Get the value from an ADC conversion +#define adcHalGetValue() ADC + +// Get the current pin being converted +#define adcHalGetCurrentPin() ((ADMUX & 0x7) | ((ADCSRB & bit(MUX5)) ? 0x80 : 0x00)) + +// Complete an ADC conversion by clearing the conversion complete flag +#define adcHalComplete() \ + do { \ + ADCSRA |= bit(ADIF); \ + } while (0); + +#endif /* __ADC_SENSOR_HAL_H__ */ \ No newline at end of file diff --git a/src/hal/sensor/airflow.cpp b/src/hal/sensor/airflow.cpp new file mode 100644 index 0000000..365d4df --- /dev/null +++ b/src/hal/sensor/airflow.cpp @@ -0,0 +1,46 @@ + +#include + +#include "../../hal/hal.h" +#include "../../hal/sensor/adc.h" +#include "../../hal/sensor/airflow.h" + +#define AIRFLOW_SENSOR_PIN 0 + +static int16_t reading; + +int airflowSensorHalInit(void) +{ + adcHalInit(); + + return HAL_OK; +} + +int airflowSensorHalFetch(void) +{ + // If the ADC is free, kick off our conversion; otherwise if its no longer + // converting our pin, take the value and free the ADC + if (!adcHalBusy()) { + adcHalBegin(AIRFLOW_SENSOR_PIN); + } else if ((adcHalGetCurrentPin() == AIRFLOW_SENSOR_PIN) && !adcHalInProgress()) { + reading = adcHalGetValue(); + adcHalComplete(); + return HAL_OK; + } + + return HAL_IN_PROGRESS; +} + +int airflowSensorHalGetValue(int16_t* value) +{ + // reading is ADC values, from PMF4003V data sheet: + // flow = (Vout - 1[V]) / 4[V] * Range = (Vout - 1[V]) / 4[V] * 20000[0.01SLM] + // Vout = ADCVal / 2^10 * 5[V] = ADCVal * 5[V] / 1024 + // So: + // flow = (ADCVal / 1024 * 5[V] - 1[V]) / 4[V] * 20000[0.01SLM] + // flow = (ADCVal * 5 - 1 * 1024) / 1024 / 4 * 20000 + // flow = (ADCVal * 5 * 20000 - 1024 * 20000) / 4096 + // flow = (ADCVal * 100000 - 20480000) / 4096 + *value = (int16_t) (((((int32_t) reading) * 100000L) - 20480000L) >> 12); + return HAL_OK; +} \ No newline at end of file diff --git a/src/hal/sensor/airflow.h b/src/hal/sensor/airflow.h new file mode 100644 index 0000000..d6df453 --- /dev/null +++ b/src/hal/sensor/airflow.h @@ -0,0 +1,18 @@ + +#ifndef __AIRFLOW_SENSOR_HAL_H__ +#define __AIRFLOW_SENSOR_HAL_H__ + +#include + +#include "../../hal/hal.h" + +// TODO: Doc +int airflowSensorHalInit(void); + +// TODO: Doc +int airflowSensorHalFetch(void); + +// TODO: Doc +int airflowSensorHalGetValue(int16_t* value); + +#endif /* __AIRFLOW_SENSOR_HAL_H__ */ \ No newline at end of file diff --git a/src/hal/sensor/pressure.cpp b/src/hal/sensor/pressure.cpp new file mode 100644 index 0000000..548e04d --- /dev/null +++ b/src/hal/sensor/pressure.cpp @@ -0,0 +1,41 @@ + +#include + +#include "../../hal/hal.h" +#include "../../hal/sensor/adc.h" +#include "../../hal/sensor/pressure.h" + +#define PRESSURE_SENSOR_PIN 1 + +static int16_t reading; + +int pressureSensorHalInit(void) +{ + adcHalInit(); + + return HAL_OK; +} + +int pressureSensorHalFetch(void) +{ + // If the ADC is free, kick off our conversion; otherwise if its no longer + // converting our pin, take the value and free the ADC + if (!adcHalBusy()) { + adcHalBegin(PRESSURE_SENSOR_PIN); + } else if ((adcHalGetCurrentPin() == PRESSURE_SENSOR_PIN) && !adcHalInProgress()) { + reading = adcHalGetValue(); + adcHalComplete(); + return HAL_OK; + } + + return HAL_IN_PROGRESS; +} + +int pressureSensorHalGetValue(int16_t* value) +{ + // For the MPXV7025 pressure sensor: (reading / 1024 - 0.5) / 0.018 = P in kPa + int32_t pascals = (((int32_t)reading * 217L) - 110995L)>>2; // convert to Pascals + int32_t u100umH2O = (pascals * 4177L)>>12; // convert Pascals to 0.1mmH2O + *value = (int16_t)u100umH2O; // return as 16 bit signed int + return HAL_OK; +} \ No newline at end of file diff --git a/src/hal/sensor/pressure.h b/src/hal/sensor/pressure.h new file mode 100644 index 0000000..c56be7e --- /dev/null +++ b/src/hal/sensor/pressure.h @@ -0,0 +1,18 @@ + +#ifndef __PRESSURE_SENSOR_HAL_H__ +#define __PRESSURE_SENSOR_HAL_H__ + +#include + +#include "../../hal/hal.h" + +// TODO: Doc +int pressureSensorHalInit(void); + +// TODO: Doc +int pressureSensorHalFetch(void); + +// TODO: Doc +int pressureSensorHalGetValue(int16_t* value); + +#endif /* __PRESSURE_SENSOR_HAL_H__ */ \ No newline at end of file diff --git a/src/hal/sensor/sensor.cpp b/src/hal/sensor/sensor.cpp deleted file mode 100644 index a703cf1..0000000 --- a/src/hal/sensor/sensor.cpp +++ /dev/null @@ -1,127 +0,0 @@ - -#include "../../hal/hal.h" -#include "../../hal/sensor/sensor.h" -#include -#include - -// Sensor pin defines -#define FLOW_SENSE_PIN A0 -#define PRESSURE_SENSE_PIN A1 - - -// The following function initializes Timer 5 to do pressure and flow sensor data collection -int sensorHalInit(void) -{ - // Sensor timer - TCCR5A=0; // set timer control registers - TCCR5B=0x0A; // set timer to clear timer on compare (CTC), and prescaler to divide by 8 - TCNT5=0; // initialize counter to 0 - OCR5A= 10000; // Set interrupt to 2khz - TIMSK5 |= 0x02; // enable timer compare interrupt - - return HAL_OK; -} - - -//-------------------------------------------------------------------------------------------------- -int16_t pSum; // pressure data accumulator -// This routine reads the pressure value as a voltage off the analog pin and inserts it into the filter -void pressureSensorHalFetch(){ - int16_t pIn; // pressure value from sensor. - pIn=analogRead(PRESSURE_SENSE_PIN); - pSum = pIn; // Filter Implementation: pSum-(pSum>>4)+pIn; // filter -} - -// This routine returns the pressure value out of the filter -int pressureSensorHalGetValue(int16_t *value){ - int32_t temp; - ATOMIC_BLOCK(ATOMIC_RESTORESTATE){ - temp = pSum; // Filter implementation: pSum>>4; - } - int32_t pascals = ((temp * 217L) - 110995L)>>2; // convert to Pascals - int32_t u100umH2O = (pascals * 4177L)>>12; // convert Pascals to 0.1mmH2O - *value = (int16_t)u100umH2O; // return as 16 bit signed int - return HAL_OK; -} - - -//-------------------------------------------------------------------------------------------------- -int16_t fSum; // flow sensor data accumulator -int32_t fVolSum; // volume accumulator, units of 0.01[mL] -// This routine reads the flow sensor value as a voltage off the analog pin and inserts it into the filter -// The flow gets integrated into the Volume accumulator -void airflowSensorHalFetch(){ - int16_t fIn; // pressure value from sensor. - fIn=analogRead(FLOW_SENSE_PIN); - fSum = fIn; // Filter Implementation: fSum-(fSum>>3)+fIn; // filter - - // get precise volume : integrate flow at every sample - int16_t f; // flow in 0.01 SLM - airflowSensorHalGetFlow(&f); - // In order to preserve precision of the volume, the acculumator should be in - // units on the order of 0.01mL, since the longest breath cycle is 6 seconds - // (5[bpm], 1:1I:E = 12[sec/breath] / 2 = 6[sec/breath] for inhalation/exhalation stages) - // Over 6 seconds, a sampling rate of 100[Hz] means 600 samples and if the calculation - // error is about 1 unit off every time, units of 0.01[mL] means a difference of only - // 6[mL] over the course of a breath (600[samples] * 0.01[mL] = 6[mL]). - // From 0.01[SLM] flow to 0.01[mL] volume: - // Vol = Flow * dt - // = (Flow[SLM] / (60[sec/min]) * (1000[0.01mL/0.01L])) * (0.01[sec]) - // = Flow * 1000 / 100 / 60 - // = Flow / 6 - // TODO: Consider using units that avoids division entirely - // TODO: floor flow at zero before using it volume? - fVolSum += f / 6; -} - -// This routine returns the flow sensor value out of the filter -int airflowSensorHalGetFlow(int16_t *value){ - int32_t temp; - ATOMIC_BLOCK(ATOMIC_RESTORESTATE){ - temp = fSum; // Filter Implementation: (fSum>>3); - } - // fSum (now temp) is filtered ADC values, from PMF4003V data sheet: - // flow = (Vout - 1[V]) / 4[V] * Range = (Vout - 1[V]) / 4[V] * 20000[0.01SLM] - // Vout = ADCVal / 2^10 * 5[V] = ADCVal * 5[V] / 1024 - // So: - // flow = (ADCVal / 1024 * 5[V] - 1[V]) / 4[V] * 20000[0.01SLM] - // flow = (ADCVal * 5 - 1 * 1024) / 1024 / 4 * 20000 - // flow = (ADCVal * 5 * 20000 - 1024 * 20000) / 4096 - // flow = (ADCVal * 100000 - 20480000 - *value = (int16_t) (((temp * 100000L) - 20480000L) >> 12); - return HAL_OK; -} - -// This routine returns the integrated volume value -int airflowSensorHalGetVolume(int16_t *value){ - ATOMIC_BLOCK(ATOMIC_RESTORESTATE){ - // Since volume is stored in units [0.01mL], convert to [mL] - *value = fVolSum / 100; - } - return HAL_OK; -} - -int airflowSensorHalResetVolume(void) { - fVolSum = 0; - return HAL_OK; -} - - -//-------------------------------------------------------------------------------------------------- -// This interrupt service routine is called every 0.5ms in order to handle timed -// events like sensor reads -// Note: -// The serial comms triggers every 100ms. -// The MPXV7025 pressure sensor has a response time of 1ms, and a warm-up time of 20ms. -// Currently sampling every 5ms and running through a /16 filter. -// The Posifa PMF4103V flow sensor has a response time of 5ms. Currently sampling every 10ms and -// running through a /8 filter -int16_t timer5Count=0; -ISR(TIMER5_COMPA_vect){ - timer5Count++; - pressureSensorHalFetch(); - if(timer5Count==2){ - airflowSensorHalFetch(); - timer5Count=0; - } -} \ No newline at end of file diff --git a/src/hal/sensor/sensor.h b/src/hal/sensor/sensor.h deleted file mode 100644 index 932761f..0000000 --- a/src/hal/sensor/sensor.h +++ /dev/null @@ -1,24 +0,0 @@ - -#ifndef __SENSOR_HAL_H__ -#define __SENSOR_HAL_H__ - -#include -#include "../../hal/hal.h" - -// Initializes the pressure and airflow sensors -int sensorHalInit(void); - -// Gets the airflow sensor value in 0.01 SLM -int airflowSensorHalGetFlow(int16_t *value); - -// Gets the pressor sensor value (measured in units of 0.1mmH2O) -int pressureSensorHalGetValue(int16_t *value); - -// Gets the volume value in ml -int airflowSensorHalGetVolume(int16_t *value); - -// Resets volume integrator -int airflowSensorHalResetVolume(void); - - -#endif /* __SENSOR_HAL_H__ */ \ No newline at end of file diff --git a/src/modules/sensors.cpp b/src/modules/sensors.cpp index 12e83eb..fcc4e76 100644 --- a/src/modules/sensors.cpp +++ b/src/modules/sensors.cpp @@ -8,9 +8,10 @@ #include "../pt/pt.h" -#include "../hal/sensor/battery.h" -#include "../hal/sensor/sensor.h" #include "../hal/timer.h" +#include "../hal/sensor/battery.h" +#include "../hal/sensor/airflow.h" +#include "../hal/sensor/pressure.h" #include "../modules/module.h" #include "../modules/control.h" @@ -39,7 +40,9 @@ // // Airflow Sensor Parameters // -#define AIRFLOW_SAMPLING_PERIOD (10 MSEC) +#define AIRFLOW_SAMPLING_PERIOD (5 MSEC) + +#define AIRFLOW_BIAS_SAMPLES 32 #define VOLUME_MINUTE_PERIOD (5 SEC) #define VOLUME_MINUTE_WINDOW (60 SEC) / VOLUME_MINUTE_PERIOD @@ -68,157 +71,211 @@ static PT_THREAD(sensorsPressureThreadMain(struct pt* pt)) PT_BEGIN(pt); // Kick off sampling timer - // TODO: refactor to use periodic timer - timerHalBegin(&pressureTimer, PRESSURE_SAMPLING_PERIOD, false); + timerHalBegin(&pressureTimer, PRESSURE_SAMPLING_PERIOD, true); - int16_t pressure; - pressureSensorHalGetValue(&pressure); // get pressure, in [0.1mmH2O] + while (1) { + // Fetch the latest sample from the sensor + PT_WAIT_UNTIL(pt, pressureSensorHalFetch() != HAL_IN_PROGRESS); + + int16_t pressure; + pressureSensorHalGetValue(&pressure); // get pressure, in [0.1mmH2O] - // Update public interface with the pressure value - sensors.currentPressure = pressure; + // Update public interface with the pressure value + sensors.currentPressure = pressure; - // Shift in new pressure - for (int i = PRESSURE_WINDOW - 1; i > 0; i--) { - previousPressure[i] = previousPressure[i - 1]; - } - previousPressure[0] = pressure; - - // Derive Peak Pressure from pressure readings; updating the public value upon - // entry into CONTROL_HOLD_IN state - if ((control.state == CONTROL_HOLD_IN) && !setPeakPressure) { - sensors.peakPressure = currentMaxPressure; - currentMaxPressure = INT16_MIN; - setPeakPressure = true; - } else { - currentMaxPressure = max(currentMaxPressure, pressure); - if (control.state == CONTROL_EXHALATION) { - setPeakPressure = false; + // Shift in new pressure + for (int i = PRESSURE_WINDOW - 1; i > 0; i--) { + previousPressure[i] = previousPressure[i - 1]; } - } - - // Derive Plateau Pressure from pressure readings during hold in; updating - // public value upon entry into CONTROL_EXHALATION state - if (control.state == CONTROL_HOLD_IN) { - plateauPressureSum += pressure; - plateauPressureSampleCount++; - } else if ((control.state == CONTROL_EXHALATION) && - (plateauPressureSampleCount > 0)) { - sensors.plateauPressure = plateauPressureSum / plateauPressureSampleCount; - plateauPressureSum = 0; - plateauPressureSampleCount = 0; - } - - // Derive PEEP Pressure from pressure readings during exhalation after values - // have "stabilized" (ie, the difference of any one point from their average - // is less than some threshold) - if (control.state == CONTROL_EXHALATION) { - int32_t sum = 0; - for (int i = 0; i < PRESSURE_WINDOW; i++) { - sum += previousPressure[i]; + previousPressure[0] = pressure; + + // Derive Peak Pressure from pressure readings; updating the public value upon + // entry into CONTROL_HOLD_IN state + if ((control.state == CONTROL_HOLD_IN) && !setPeakPressure) { + sensors.peakPressure = currentMaxPressure; + currentMaxPressure = INT16_MIN; + setPeakPressure = true; + } else { + currentMaxPressure = max(currentMaxPressure, pressure); + if (control.state == CONTROL_EXHALATION) { + setPeakPressure = false; + } } - int16_t average = sum / PRESSURE_WINDOW; + // Derive Plateau Pressure from pressure readings during hold in; updating + // public value upon entry into CONTROL_EXHALATION state + if (control.state == CONTROL_HOLD_IN) { + plateauPressureSum += pressure; + plateauPressureSampleCount++; + } else if ((control.state == CONTROL_EXHALATION) && + (plateauPressureSampleCount > 0)) { + sensors.plateauPressure = plateauPressureSum / plateauPressureSampleCount; + plateauPressureSum = 0; + plateauPressureSampleCount = 0; + } - bool flat = true; - for (int i = 0; i < PRESSURE_WINDOW; i++) { - if (abs(average - previousPressure[i]) > PEEP_PRESSURE_FLAT_THRESHOLD) { - flat = false; + // Derive PEEP Pressure from pressure readings during exhalation after values + // have "stabilized" (ie, the difference of any one point from their average + // is less than some threshold) + if (control.state == CONTROL_EXHALATION) { + int32_t sum = 0; + for (int i = 0; i < PRESSURE_WINDOW; i++) { + sum += previousPressure[i]; + } + + int16_t average = sum / PRESSURE_WINDOW; + + bool flat = true; + for (int i = 0; i < PRESSURE_WINDOW; i++) { + if (abs(average - previousPressure[i]) > PEEP_PRESSURE_FLAT_THRESHOLD) { + flat = false; + } + } + + // TODO: determine if we want this reading to update so long as it can, or + // measure once per breath, like other derived pressures + if (flat) { + sensors.peepPressure = pressure; } } - // TODO: determine if we want this reading to update so long as it can, or - // measure once per breath, like other derived pressures - if (flat) { - sensors.peepPressure = pressure; + // Derive inhalation detection from pressure readings during exhalation by + // looking for a dip in pressure below the PEEP threshold (or below an + // absolute pressure threshold) + if ((control.state == CONTROL_EXHALATION) && !sensors.inhalationDetected) { + if ((pressure < INHALATION_DETECTION_ABSOLUTE_THRESHOLD) || + (sensors.peepPressure - pressure > INHALATION_DETECTION_PEEP_THRESHOLD)) { + sensors.inhalationDetected = true; + inhalationTimeout = 0; + } } - } - - // Derive inhalation detection from pressure readings during exhalation by - // looking for a dip in pressure below the PEEP threshold (or below an - // absolute pressure threshold) - if ((control.state == CONTROL_EXHALATION) && !sensors.inhalationDetected) { - if ((pressure < INHALATION_DETECTION_ABSOLUTE_THRESHOLD) || - (sensors.peepPressure - pressure > INHALATION_DETECTION_PEEP_THRESHOLD)) { - sensors.inhalationDetected = true; - inhalationTimeout = 0; + if (sensors.inhalationDetected && + (inhalationTimeout++ == (uint8_t) (INHALATION_TIMEOUT / PRESSURE_SAMPLING_PERIOD))) { + sensors.inhalationDetected = false; } - } - if (sensors.inhalationDetected && - (inhalationTimeout++ == (uint8_t) (INHALATION_TIMEOUT / PRESSURE_SAMPLING_PERIOD))) { - sensors.inhalationDetected = false; + + DEBUG_PRINT_EVERY(100, "Pressure = %c%u.%01u mmH2O", + (pressure < 0) ? '-' : ' ', abs(pressure)/10, abs(pressure)%10); + + PT_WAIT_UNTIL(pt, timerHalRun(&pressureTimer) != HAL_IN_PROGRESS); } - DEBUG_PRINT_EVERY(200, "Pressure = %c%u.%01u mmH2O", - (pressure < 0) ? '-' : ' ', abs(pressure)/10, abs(pressure)%10); - - PT_WAIT_UNTIL(pt, timerHalRun(&pressureTimer) != HAL_IN_PROGRESS); - PT_RESTART(pt); + // Should never reach here PT_END(pt); } static PT_THREAD(sensorsAirFlowThreadMain(struct pt* pt)) { + static int32_t airflowSum = 0; + static uint32_t previousSampleTime = 0; static bool setVolumeIn = false; static int16_t volumeMinuteWindow[VOLUME_MINUTE_WINDOW] = {0}; static uint8_t volumeMinuteTimeout = 0; static int16_t maxVolume = INT16_MIN; static bool volumeReset = false; - + static int16_t airflowBias = 0; + static int airflowBiasCounter = AIRFLOW_BIAS_SAMPLES; + PT_BEGIN(pt); - // Kick off sampling timer - // TODO: refactor to use periodic timer - timerHalBegin(&airflowTimer, AIRFLOW_SAMPLING_PERIOD, false); - - int16_t airflow; - int16_t airvolume; - airflowSensorHalGetFlow(&airflow); // get airflow, in [0.1SLM] - airflowSensorHalGetVolume(&airvolume); // get airvolume, in [mL] (derived from volume) - - // Update public interface with the flow and volume values - sensors.currentFlow = airflow; - sensors.currentVolume = airvolume; - - // Derive Volume IN from current volume; updating the public value upon entry - // into CONTROL_HOLD_IN state - if ((control.state == CONTROL_HOLD_IN) && !setVolumeIn) { - sensors.volumeIn = airvolume; - setVolumeIn = true; - } else if (control.state == CONTROL_EXHALATION) { - setVolumeIn = false; + // Find the bias of the sensor + while (airflowBiasCounter--) { + PT_WAIT_UNTIL(pt, airflowSensorHalFetch() != HAL_IN_PROGRESS); + int16_t airflow; + airflowSensorHalGetValue(&airflow); + airflowSum += airflow; } - // Volume OUT cannot be derived from flow sensor due to position and direction + airflowBias = airflowSum / AIRFLOW_BIAS_SAMPLES; + airflowSum = 0; - // Derive Volume/min from current volume by remembering the max volume over - // last minute volume period and shifting it into the window, updating the - // minute volume by subtracting out the old value and adding in the new one - maxVolume = max(maxVolume, airvolume); - if (volumeMinuteTimeout++ == (uint8_t) (VOLUME_MINUTE_PERIOD / AIRFLOW_SAMPLING_PERIOD)) { - sensors.volumePerMinute -= volumeMinuteWindow[VOLUME_MINUTE_WINDOW - 1]; - sensors.volumePerMinute += maxVolume; - for (int i = VOLUME_MINUTE_WINDOW - 1; i > 0; i--) { - volumeMinuteWindow[i] = volumeMinuteWindow[i - 1]; - } - volumeMinuteWindow[0] = maxVolume; - maxVolume = INT16_MIN; - } + DEBUG_PRINT("Airflow bias = %c%u.%02u SLM", + (airflowBias < 0) ? '-' : ' ', abs(airflowBias)/100, abs(airflowBias)%100); + + // Kick off sampling timer + timerHalBegin(&airflowTimer, AIRFLOW_SAMPLING_PERIOD, true); - // Determine if its time to reset the volume integrator, do it once in exhalation - if ((control.state == CONTROL_EXHALATION) && !volumeReset) { - DEBUG_PRINT("*** Reset Volume ***"); - airflowSensorHalResetVolume(); - volumeReset = true; - } else if (control.state == CONTROL_INHALATION) { - volumeReset = false; + while (1) { + // Fetch the latest sample from the sensor + PT_WAIT_UNTIL(pt, airflowSensorHalFetch() != HAL_IN_PROGRESS); + + // Calculate dt from the current time from the ideal sample time against + // the previous amount off of the ideal sample time + uint32_t currentSampleTime; + uint32_t dt; + + currentSampleTime = timerHalCurrent(&airflowTimer); + dt = AIRFLOW_SAMPLING_PERIOD + currentSampleTime - previousSampleTime; + previousSampleTime = currentSampleTime; + + int16_t airflow; + int16_t airvolume; + airflowSensorHalGetValue(&airflow); // get airflow, in [0.1SLM] + + // Apply the bias to the flow reading + airflow -= airflowBias; + + // Integrate the airflow to get the volume + // In order to preserve precision of the volume, the acculumator should be in + // units on the order of 0.001mL, since the longest breath cycle is 6 seconds + // (5[bpm], 1:1I:E = 12[sec/breath] / 2 = 6[sec/breath] for inhalation/exhalation stages) + // Over 6 seconds, a sampling rate of 200[Hz] means 1200 samples and if the calculation + // error is about 1 unit off every time, units of 0.001[mL] means a difference of only + // 1.2[mL] over the course of a breath (1200[samples] * 0.001[mL] = 1.2[mL]). + // From 0.01[SLM] flow to 0.001[mL] volume: + // Vol = Flow * dt + // = (Flow[SLM] / (60[sec/min]) * (10000[0.001mL/0.01L])) * ((dt[usec]) / (1000000[usec/sec])) + // = Flow * dt * 10000 / 1000000 / 60 + // = Flow * dt / 6000 + airflowSum += ((int32_t) airflow) * ((int32_t) dt) / 6000L; // airflowSum in [0.001mL] + airvolume = airflowSum / 1000L; // airflow in [mL] + + // Update public interface with the flow and volume values + sensors.currentFlow = airflow; + sensors.currentVolume = airvolume; + + // Derive Volume IN from current volume; updating the public value upon entry + // into CONTROL_HOLD_IN state + if ((control.state == CONTROL_HOLD_IN) && !setVolumeIn) { + sensors.volumeIn = airvolume; + setVolumeIn = true; + } else if (control.state == CONTROL_EXHALATION) { + setVolumeIn = false; + } + + // Volume OUT cannot be derived from flow sensor due to position and direction + + // Derive Volume/min from current volume by remembering the max volume over + // last minute volume period and shifting it into the window, updating the + // minute volume by subtracting out the old value and adding in the new one + maxVolume = max(maxVolume, airvolume); + if (volumeMinuteTimeout++ == (uint8_t) (VOLUME_MINUTE_PERIOD / AIRFLOW_SAMPLING_PERIOD)) { + sensors.volumePerMinute -= volumeMinuteWindow[VOLUME_MINUTE_WINDOW - 1]; + sensors.volumePerMinute += maxVolume; + for (int i = VOLUME_MINUTE_WINDOW - 1; i > 0; i--) { + volumeMinuteWindow[i] = volumeMinuteWindow[i - 1]; + } + volumeMinuteWindow[0] = maxVolume; + maxVolume = INT16_MIN; + } + + // Determine if its time to reset the volume integrator, do it once in exhalation + if ((control.state == CONTROL_EXHALATION) && !volumeReset) { + DEBUG_PRINT("*** Reset Volume ***"); + airflowSum = 0; + volumeReset = true; + } else if (control.state == CONTROL_INHALATION) { + volumeReset = false; + } + + DEBUG_PRINT_EVERY(200, "Airflow = %c%u.%02u SLM", + (airflow < 0) ? '-' : ' ', abs(airflow)/100, abs(airflow)%100); + DEBUG_PRINT_EVERY(200, "AirVolume = %d mL", airvolume); + + PT_WAIT_UNTIL(pt, timerHalRun(&airflowTimer) != HAL_IN_PROGRESS); } - - DEBUG_PRINT_EVERY(200, "Airflow = %c%u.%02u SLM", - (airflow < 0) ? '-' : ' ', abs(airflow)/100, abs(airflow)%100); - DEBUG_PRINT_EVERY(200, "AirVolume = %d mL", airvolume); - - PT_WAIT_UNTIL(pt, timerHalRun(&airflowTimer) != HAL_IN_PROGRESS); - PT_RESTART(pt); + + // Should never reach here PT_END(pt); } @@ -255,7 +312,10 @@ PT_THREAD(sensorsThreadMain(struct pt* pt)) int sensorsModuleInit(void) { // TODO: Improve error propagation for all hal init failures - if (sensorHalInit() != HAL_OK) { + if (pressureSensorHalInit() != HAL_OK) { + return MODULE_FAIL; + } + if (airflowSensorHalInit() != HAL_OK) { return MODULE_FAIL; } if (batterySensorHalInit() != HAL_OK) { From 8ae2b0d9b62a4ff0a77b9513b1565983a3ae981d Mon Sep 17 00:00:00 2001 From: "Rosen, Michael R" Date: Wed, 29 Apr 2020 19:04:24 -0700 Subject: [PATCH 10/15] sensors: add safety yields to sensor threads Signed-off-by: Rosen, Michael R --- src/modules/sensors.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/modules/sensors.cpp b/src/modules/sensors.cpp index fcc4e76..992d095 100644 --- a/src/modules/sensors.cpp +++ b/src/modules/sensors.cpp @@ -157,6 +157,9 @@ static PT_THREAD(sensorsPressureThreadMain(struct pt* pt)) DEBUG_PRINT_EVERY(100, "Pressure = %c%u.%01u mmH2O", (pressure < 0) ? '-' : ' ', abs(pressure)/10, abs(pressure)%10); + // Ensure this threads cannot block if it somehow elapses the timer too fast + PT_YIELD(pt); + PT_WAIT_UNTIL(pt, timerHalRun(&pressureTimer) != HAL_IN_PROGRESS); } @@ -270,7 +273,10 @@ static PT_THREAD(sensorsAirFlowThreadMain(struct pt* pt)) DEBUG_PRINT_EVERY(200, "Airflow = %c%u.%02u SLM", (airflow < 0) ? '-' : ' ', abs(airflow)/100, abs(airflow)%100); - DEBUG_PRINT_EVERY(200, "AirVolume = %d mL", airvolume); + DEBUG_PRINT_EVERY(200, "AirVolume = %d mL", airvolume); + + // Ensure this threads cannot block if it somehow elapses the timer too fast + PT_YIELD(pt); PT_WAIT_UNTIL(pt, timerHalRun(&airflowTimer) != HAL_IN_PROGRESS); } From 0de6888345d7ee1920215b5e3e277ae321eece70 Mon Sep 17 00:00:00 2001 From: "Rosen, Michael R" Date: Thu, 30 Apr 2020 15:51:35 -0700 Subject: [PATCH 11/15] debug: add debug plotter mechanism In order to take advantage of the Arduino Serial Plotter, debug messages mechanism had to be slightly retuned and now the debug feature includes a way to plot values with some limitations. Signed-off-by: Rosen, Michael R --- src/modules/control.cpp | 4 ++-- src/modules/sensors.cpp | 3 +++ src/util/debug.h | 44 +++++++++++++++++++++++++++++++++++++++++ 3 files changed, 49 insertions(+), 2 deletions(-) diff --git a/src/modules/control.cpp b/src/modules/control.cpp index 688c55a..0be6fa4 100644 --- a/src/modules/control.cpp +++ b/src/modules/control.cpp @@ -16,7 +16,7 @@ #include "../util/metrics.h" -//#define DEBUG +#define DEBUG #define DEBUG_MODULE "control" #include "../util/debug.h" @@ -221,7 +221,7 @@ static int updateControl(void) metricsStop(&midControlTiming); DEBUG_PRINT_EVERY(100,"Control Stats: Avg us: %u\n",midControlTiming.average); - + DEBUG_PLOT((int32_t)flowSensorInput, (int32_t)targetAirFlow, (int32_t)controlOutLimited, (int32_t)(Kp*controlP), (int32_t)(Ki*controlD)); // Return unfinished return 0; } diff --git a/src/modules/sensors.cpp b/src/modules/sensors.cpp index 992d095..ffb4e64 100644 --- a/src/modules/sensors.cpp +++ b/src/modules/sensors.cpp @@ -310,6 +310,9 @@ PT_THREAD(sensorsThreadMain(struct pt* pt)) if (!PT_SCHEDULE(sensorsBatteryThreadMain(&sensorsBatteryThread))) { PT_EXIT(pt); } + + // TODO: Mess with the units to make the graph scale nicely? + DEBUG_PLOT(sensors.currentFlow, sensors.currentVolume, sensors.currentPressure); PT_RESTART(pt); PT_END(pt); diff --git a/src/util/debug.h b/src/util/debug.h index fef2765..80f099f 100644 --- a/src/util/debug.h +++ b/src/util/debug.h @@ -1,5 +1,8 @@ #include +#include +#include +#include #include @@ -15,10 +18,49 @@ #define DEBUG_SERIAL_PORT Serial #endif +// In order to enable the standard Arduino Serial Plotter, all other types of +// messages must be disabled and only a single module can plot a single set +// of variables at one time. As such, to use the DEBUG_PLOT macro, define +// DEBUG_PLOTTING here with the module name (DEBUG_MODULE) to be plotted, +// example commented out below: +// #define DEBUG_PLOTTING "control" + // Initial setup of debug; only to be used once #define DEBUG_BEGIN DEBUG_SERIAL_PORT.begin(230400) #ifdef DEBUG +#ifdef DEBUG_PLOTTING + +// Use DEBUG_PLOT to plot a set of variables, can only be used once in a module +// and requires DEBUG_PLOTTING to be defined and set the correct module's name, +// see above +// NOTE: Compiler seems pretty good at optimizing out the strcmp, would like a +// better way of doing this but the rest of the mechanism is so clean, I +// am relying on the compiler to do the optimization rather than via some +// macros somehow... +#define DEBUG_PLOT(...) \ + do { \ + static bool header = true; \ + if (strcmp(DEBUG_PLOTTING, DEBUG_MODULE) == 0) { \ + if (header) { \ + DEBUG_SERIAL_PORT.println(#__VA_ARGS__); \ + header = false; \ + } \ + int32_t _vars[] = { __VA_ARGS__ }; \ + for (int i = 0; i < sizeof(_vars) / sizeof(int32_t); i++) { \ + if (i > 0) DEBUG_SERIAL_PORT.print(","); \ + DEBUG_SERIAL_PORT.print(_vars[i]); \ + } \ + DEBUG_SERIAL_PORT.print("\n"); \ + } \ + } while (0); \ + +#define DEBUG_PRINT(...) +#define DEBUG_PRINT_EVERY(...) + +#else + +#define DEBUG_PLOT(...) // Use DEBUG_PRINT to add print messages like printf #define DEBUG_PRINT(...) \ @@ -41,7 +83,9 @@ } \ } while (0); +#endif #else +#define DEBUG_PLOT(...) #define DEBUG_PRINT(...) #define DEBUG_PRINT_EVERY(...) #endif \ No newline at end of file From 006ba265cf55a602b321b0fe3a84729f7eb7ac0b Mon Sep 17 00:00:00 2001 From: JanS Date: Thu, 30 Apr 2020 16:40:46 +0200 Subject: [PATCH 12/15] add initial controller scaling added scaling factor to trajectory and motor output according to Andrews measurements. reset I-integrator for every cycle --- src/modules/control.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/src/modules/control.cpp b/src/modules/control.cpp index 0be6fa4..8eb60e1 100644 --- a/src/modules/control.cpp +++ b/src/modules/control.cpp @@ -67,6 +67,10 @@ static float nomVelocity = 0.0f; static uint32_t breathTimerStateStart = 0; static struct metrics midControlTiming; +static float controlI=0.0f; + +//this is our initial guess scaling factor to match the trajectory to flow sensor values +#define SCALING_FACTOR 1.89f // Pre-compute the trajectory based on known set points and timing requirements static void computeTrajectory() @@ -84,7 +88,7 @@ static void computeTrajectory() rampTime = (totalBreathTime - targetInhalationTime) / (2.0f + trapezoidRatio); platTime = trapezoidRatio * rampTime; nomVelocity = abs((float)targetPosition - (float)currentPosition) * 1000.0f * 1000000.0f * 60.0f / ((float)(rampTime + platTime) * 360.0f); - + nomVelocity/=SCALING_FACTOR; } else if (control.state == CONTROL_BEGIN_INHALATION) { @@ -93,6 +97,7 @@ static void computeTrajectory() rampTime = targetInhalationTime / (2.0f + trapezoidRatio); platTime = trapezoidRatio * rampTime; nomVelocity = abs((float)targetPosition - (float)currentPosition) * 1000.0f * 1000000.0f * 60.0f / ((float)(rampTime + platTime) * 360.0f); + nomVelocity/=SCALING_FACTOR; } else { // Blank @@ -140,14 +145,13 @@ static int updateControl(void) float flowSensorInput; static float lastFlowSensorInput=0.0f; float controlP,controlD; - static float controlI=0.0f; float controlOut; float controlOutLimited; //TEMPORARY ASSIGNMENT. SHOULD BE STORED IN PARAMETERS float Kf=0.8f; float Kp=0.1f; - float Ki=.0f; + float Ki=.01f; float Kd=0.1f; float KiMax=10.0f; //max speed adjustment because of I-part in % of nominalVelocity float controlMax=20.0f; //max speed adjustment of current target velocity @@ -213,6 +217,8 @@ static int updateControl(void) //final control output is feed forward + limited controlOut controlOutLimited=Kf*targetAirFlow+controlOut; + controlOutLimited*=SCALING_FACTOR; + // Send motor commands motorHalCommand(targetPosition, controlOutLimited); @@ -221,7 +227,9 @@ static int updateControl(void) metricsStop(&midControlTiming); DEBUG_PRINT_EVERY(100,"Control Stats: Avg us: %u\n",midControlTiming.average); - DEBUG_PLOT((int32_t)flowSensorInput, (int32_t)targetAirFlow, (int32_t)controlOutLimited, (int32_t)(Kp*controlP), (int32_t)(Ki*controlD)); + + DEBUG_PLOT((int32_t)flowSensorInput, (int32_t)targetAirFlow, (int32_t)controlOutLimited, (int32_t)(Kp*controlP), (int32_t)(Kd*controlD), (int32_t)(Ki*controlD)); + // Return unfinished return 0; } @@ -288,6 +296,7 @@ static PT_THREAD(controlThreadMain(struct pt* pt)) // Compute trajectory computeTrajectory(); + controlI=0.0f; //reset I-part control.state = CONTROL_INHALATION; } else if (control.state == CONTROL_INHALATION) { From c9369146dc57a0d83b1cabdc86c2a6f173c94368 Mon Sep 17 00:00:00 2001 From: JanS Date: Fri, 1 May 2020 18:09:53 +0200 Subject: [PATCH 13/15] First controller tuning... Dirty fix to motor target position initial controller tuning - works, but looses steps and can't compensate (yet) --- src/modules/control.cpp | 45 +++++++++++++++++++++++++++-------------- 1 file changed, 30 insertions(+), 15 deletions(-) diff --git a/src/modules/control.cpp b/src/modules/control.cpp index 8eb60e1..a174a14 100644 --- a/src/modules/control.cpp +++ b/src/modules/control.cpp @@ -68,6 +68,8 @@ static uint32_t breathTimerStateStart = 0; static struct metrics midControlTiming; static float controlI=0.0f; +static float controlOutputFiltered=0.0f; + //this is our initial guess scaling factor to match the trajectory to flow sensor values #define SCALING_FACTOR 1.89f @@ -84,7 +86,7 @@ static void computeTrajectory() if (control.state == CONTROL_BEGIN_EXHALATION) { // Position information needs to be integrated to allow for adaptive positioning on exhalation - targetPosition = 5; + targetPosition = 20; rampTime = (totalBreathTime - targetInhalationTime) / (2.0f + trapezoidRatio); platTime = trapezoidRatio * rampTime; nomVelocity = abs((float)targetPosition - (float)currentPosition) * 1000.0f * 1000000.0f * 60.0f / ((float)(rampTime + platTime) * 360.0f); @@ -149,12 +151,12 @@ static int updateControl(void) float controlOutLimited; //TEMPORARY ASSIGNMENT. SHOULD BE STORED IN PARAMETERS - float Kf=0.8f; - float Kp=0.1f; - float Ki=.01f; - float Kd=0.1f; - float KiMax=10.0f; //max speed adjustment because of I-part in % of nominalVelocity - float controlMax=20.0f; //max speed adjustment of current target velocity + float Kf=0.9f; + float Kp=1.0f; + float Kd=0.8f; + float Ki=.25f; + float KiMax=80.0f; //max speed adjustment because of I-part in % of nominalVelocity + float controlMax=100.0f; //max speed adjustment of current target velocity if (control.state == CONTROL_EXHALATION) @@ -201,34 +203,46 @@ static int updateControl(void) controlI=(nomVelocity*KiMax/100.0f)/Ki; if ((Ki*controlI)<(-nomVelocity*KiMax/100.0f)) - controlI=-(nomVelocity*KiMax/100.0f)/Ki; + controlI=-(nomVelocity*KiMax/100.0f)/Ki; //limit and output controlOut=Kp*controlP+Kd*controlD+Ki*controlI; - if (controlOut>(controlMax/100.0f*targetAirFlow)) - controlOut=(controlMax/100.0f*targetAirFlow); + if (controlOut>(controlMax/100.0f*nomVelocity)) + controlOut=(controlMax/100.0f*nomVelocity); - if (controlOut<-(controlMax/100.0f*targetAirFlow)) - controlOut=-(controlMax/100.0f*targetAirFlow); + if (controlOut<-(controlMax/100.0f*nomVelocity)) + controlOut=-(controlMax/100.0f*nomVelocity); //final control output is feed forward + limited controlOut controlOutLimited=Kf*targetAirFlow+controlOut; controlOutLimited*=SCALING_FACTOR; - // Send motor commands - motorHalCommand(targetPosition, controlOutLimited); + //IIR filter + controlOutputFiltered=0.9f*controlOutputFiltered+0.1f*controlOutLimited; + + //TODO: Remove this diry fix here. It needs to be replace with a better trajectory planning. + if (control.state == CONTROL_EXHALATION) + { + // Send motor commands + motorHalCommand(targetPosition, controlOutputFiltered); + }else + { + // Send motor commands + motorHalCommand(targetPosition*3, controlOutputFiltered); + } + lastFlowSensorInput=flowSensorInput; metricsStop(&midControlTiming); DEBUG_PRINT_EVERY(100,"Control Stats: Avg us: %u\n",midControlTiming.average); - DEBUG_PLOT((int32_t)flowSensorInput, (int32_t)targetAirFlow, (int32_t)controlOutLimited, (int32_t)(Kp*controlP), (int32_t)(Kd*controlD), (int32_t)(Ki*controlD)); + DEBUG_PLOT((int32_t)flowSensorInput, (int32_t)targetAirFlow, (int32_t)controlOutLimited, (int32_t)(Kp*controlP), (int32_t)controlOutputFiltered, (int32_t)(Ki*controlD)); // Return unfinished return 0; @@ -297,6 +311,7 @@ static PT_THREAD(controlThreadMain(struct pt* pt)) computeTrajectory(); controlI=0.0f; //reset I-part + controlOutputFiltered=0.0f; control.state = CONTROL_INHALATION; } else if (control.state == CONTROL_INHALATION) { From 93f9c305d50fb5d39dee14338cca5394ed4abb16 Mon Sep 17 00:00:00 2001 From: "Rosen, Michael R" Date: Fri, 1 May 2020 09:39:07 -0700 Subject: [PATCH 14/15] control: minor fixes to control module . Add macro to enable/disable closed loop control while under development . Fix typo in timeout check for exhalation Signed-off-by: Rosen, Michael R --- src/modules/control.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/modules/control.cpp b/src/modules/control.cpp index a174a14..4b1d6b9 100644 --- a/src/modules/control.cpp +++ b/src/modules/control.cpp @@ -28,6 +28,9 @@ #define MIN_VELOCITY 2 // TODO: Fix this +// Uncomment the following to enable the current closed loop control +// #define CONTROL_CLOSED_LOOP + // Public Variables struct control control = { @@ -151,10 +154,18 @@ static int updateControl(void) float controlOutLimited; //TEMPORARY ASSIGNMENT. SHOULD BE STORED IN PARAMETERS + // TODO: Currently, enable open loop control if needed, disabling closed loop while in development +#ifdef CONTROL_CLOSED_LOOP float Kf=0.9f; float Kp=1.0f; float Kd=0.8f; float Ki=.25f; +#else + float Kf=1.0f; + float Kp=0.0f; + float Kd=0.0f; + float Ki=0.0f; +#endif float KiMax=80.0f; //max speed adjustment because of I-part in % of nominalVelocity float controlMax=100.0f; //max speed adjustment of current target velocity @@ -380,7 +391,7 @@ static PT_THREAD(controlThreadMain(struct pt* pt)) // next control cycle; otherwise move on to the next steps in exhalation // TODO: Consider what a timeout here means, it means the motor wasn't // able to get all the way back, should we really keep going? - if (!controlComplete && !checkInhalationTimeout()) { + if (!controlComplete && !checkExhalationTimeout()) { PT_WAIT_UNTIL(pt, timerHalRun(&controlTimer) != HAL_IN_PROGRESS); } else { break; From 702df522e8e68a8f1c5be3b85d82f251031806ee Mon Sep 17 00:00:00 2001 From: Inaky Perez-Gonzalez Date: Sat, 2 May 2020 16:39:48 -0700 Subject: [PATCH 15/15] DEBUG_PLOT: add millisecond stamping When using DEBUG_PLOT, prefix each line with a millisecond stamp (since boot). Signed-off-by: Inaky Perez-Gonzalez --- CONTRIBUTING | 23 +++++++++++++++++++++++ src/util/debug.h | 5 ++++- 2 files changed, 27 insertions(+), 1 deletion(-) diff --git a/CONTRIBUTING b/CONTRIBUTING index 1fa2a8f..2912965 100644 --- a/CONTRIBUTING +++ b/CONTRIBUTING @@ -26,3 +26,26 @@ Bug fixes - Urgent bug fixes may be done from ‘master’ while development has continued on ‘develop’ - In this case, check out a bug fix branch from ‘master’ - Merge the fix back into ‘master’ AND ‘develop’ + + +Plotting + +- if you #define src/util/debug.h:DEBUG_PLOTTING to "control" + (real) or "sensors" (simulated values), the controller will print + sampled data to the debug port in the following format:: + + ms,FIELDA,FIELDB,...\r + MS1,VALA1,VALB1,...\r + MS2,VALA2,VALB2,...\r + MS3,VALA3,VALB3,...\r + + Where the first line are the header names with ms being timestamps + in milliseconds. + + This can be visualized with the arduino plotter or with tools such + as kst2 (eg: in Linux -- note you might have to edit the beggining + of the file to remove any leftover output before flashing until the + headers so kst2 can parse it correctly):: + + $ cat /dev/ttyACM0 > file.txt + $ kst2 file.txt diff --git a/src/util/debug.h b/src/util/debug.h index 80f099f..2c49e1b 100644 --- a/src/util/debug.h +++ b/src/util/debug.h @@ -5,6 +5,7 @@ #include #include +#include /* millis() */ #ifndef DEBUG_MODULE #error "Cannot include debug.h without defining DEBUG_MODULE" @@ -43,9 +44,11 @@ static bool header = true; \ if (strcmp(DEBUG_PLOTTING, DEBUG_MODULE) == 0) { \ if (header) { \ - DEBUG_SERIAL_PORT.println(#__VA_ARGS__); \ + DEBUG_SERIAL_PORT.println("ms," #__VA_ARGS__); \ header = false; \ } \ + DEBUG_SERIAL_PORT.print(millis()); \ + DEBUG_SERIAL_PORT.print(','); \ int32_t _vars[] = { __VA_ARGS__ }; \ for (int i = 0; i < sizeof(_vars) / sizeof(int32_t); i++) { \ if (i > 0) DEBUG_SERIAL_PORT.print(","); \