/* * File: LCCV_Operations.c * Author: NM * * Created on 9 février 2015, 16:12 */ #include "GlobalConstants.h" #include "RamInitialisation.h" #include "CAL/CAL_Module1.h" #include #include #include #include "HAL/HAL_TMR.h" #include "HAL/HAL_IO.h" #include "LCCV/LCCV_Acquisition.h" #include "LCCV/LCCV_Operations.h" #include "IHM/IHM_LCD_Controller.h" #include "LCCV/LCCV_Tools.h" #include "HAL/HAL_ADC.h" const float highPassFilterFrequency[ MODE4_FREQUENCY_RANGE_NB ] = { 20, // Surface sensor : not tighted 20, // Surface sensor : middle range 20, // Surface sensor : upper range 700, // Generic sensor : SG1 1000 // Generic sensor : production }; const float lowPassFilterFrequency[ MODE4_FREQUENCY_RANGE_NB ] = { 300, // Surface sensor : not tighted 400, // Surface sensor : middle range 500, // Surface sensor : upper range 2500, // Generic sensor : SG1 2800 // Generic sensor : production }; //------------------------------------------------------------------------------------ // LCCV_CoilsResistanceMeasure() //------------------------------------------------------------------------------------ // @brief Measurement of the coils resistances // see LCCV specification : MODE 1 measurement // // @noparam // // @return Code de retour: // @retval none // // // @nothrow //------------------------------------------------------------------------------------ int LCCV_CoilsResistanceMeasure( float *aCoil1Resistance, float *aCoil2Resistance ) { int retCode; float pulseVoltage; float coilVoltage; float current; float m1Gain; // Clear the screen except the title IHM_LCD_ClearDataLines(); // Calibrate analog conditionner retCode = LCCV_SetCalibration(); if( retCode != RC_OK ) return retCode; // Set the M1 gain for current operation retCode = CAL_SetM1GainValue( MODE1_M1_GAIN, &m1Gain ); if( retCode != RC_OK ) return retCode; else if( m1Gain > MODE1_M1_GAIN + MODE1_M1_GAIN_TOLERANCE || m1Gain < MODE1_M1_GAIN - MODE1_M1_GAIN_TOLERANCE ) return RC_ERROR_WRONG_M1_GAIN; #ifdef _TRACE_COIL1_RESISTANCE _DEBUG_INIT() char buffer[LCD_CHAR_BY_LINE + 1]; sprintf(buffer, "M1 gain = %.2f", m1Gain ); _DEBUG_TRACE( strlen(buffer), buffer ) sprintf(buffer, "A11 = %.4f", _M1_COEFF_A11 ); _DEBUG_TRACE( strlen(buffer), buffer ) sprintf(buffer, "Drift = %.4f", _M1_DRIFT ); _DEBUG_TRACE( strlen(buffer), buffer ) #endif //----------------------------------------------------------------- // Select coil 1 retCode = HAL_IO_SetSsrResistance1Measure(); if( retCode != RC_OK ) return retCode; // Send a continuous voltage stimulus HAL_IO_SendContinuousBtStimulus(); // Measurement of stimulus voltage retCode = LCCV_GetLowPulseVoltage( &pulseVoltage ); if( retCode != RC_OK ) return retCode; #ifdef _TRACE_COIL1_RESISTANCE sprintf(buffer, "Pulse = %.4f V", pulseVoltage ); _DEBUG_TRACE( strlen(buffer), buffer ) #endif // Measurement of coil 1 voltage retCode = LCCV_GetCoilVoltage( &coilVoltage ); if( retCode != RC_OK ) return retCode; #ifdef _TRACE_COIL1_RESISTANCE // Used for multimeter measurement HAL_TMR_Delay_ms( 1000 ); HAL_TMR_Delay_ms( 1000 ); sprintf(buffer, "Coil1 = %.4f V", coilVoltage ); _DEBUG_TRACE( strlen(buffer), buffer ) #endif // Stop the stimulus HAL_IO_StopContinuousBtStimulus(); // Coil voltage out of max range if( coilVoltage > MAX_COIL_VOLTAGE ) { // Open circuit or insufficient load *aCoil1Resistance = COIL_RESISTOR_OPEN_CIRCUIT; } // Coil voltage out of min range else if( coilVoltage < MIN_COIL_VOLTAGE ) { // Short circuit or too heavy load *aCoil1Resistance = COIL_RESISTOR_SHORT_CIRCUIT; } else { // Compute the coil 1 current current = (pulseVoltage - coilVoltage) / (float)COIL_1_LIMITING_RESISTOR; // Compute the coil 1 resistance *aCoil1Resistance = coilVoltage / current; } #ifdef _TRACE_COIL1_RESISTANCE sprintf(buffer, "I1 = %.3f mA", current*1000 ); _DEBUG_TRACE( strlen(buffer), buffer ) sprintf(buffer, "R1 = %.3f ohm", *aCoil1Resistance ); _DEBUG_TRACE( strlen(buffer), buffer ) #endif //----------------------------------------------------------------- // Select coil 2 retCode = HAL_IO_SetSsrResistance2Measure(); if( retCode != RC_OK ) return retCode; // Send a continuous voltage stimulus HAL_IO_SendContinuousBtStimulus(); // Measurement of stimulus voltage retCode = LCCV_GetLowPulseVoltage( &pulseVoltage ); if( retCode != RC_OK ) return retCode; // Measurement of coil 2 voltage retCode = LCCV_GetCoilVoltage( &coilVoltage ); if( retCode != RC_OK ) return retCode; // Stop the stimulus HAL_IO_StopContinuousBtStimulus(); // Coil voltage out of max range if( coilVoltage > MAX_COIL_VOLTAGE ) { // Open circuit or insufficient load *aCoil2Resistance = COIL_RESISTOR_OPEN_CIRCUIT; } // Coil voltage out of min range else if( coilVoltage < MIN_COIL_VOLTAGE ) { // Short circuit or too heavy load *aCoil2Resistance = COIL_RESISTOR_SHORT_CIRCUIT; } else { // Compute the coil 2 current current = (pulseVoltage - coilVoltage) / COIL_2_LIMITING_RESISTOR; // Compute the coil 2 resistance *aCoil2Resistance = coilVoltage / current; } // Disable all SSR of the analog conditionner DISABLE_ALL_SSR(); return RC_OK; } //------------------------------------------------------------------------------------ // LCCV_ModulatedDampedMode() //------------------------------------------------------------------------------------ // @brief Measurement mode with modulated stimulation // see LCCV specification : MODE 3 // // @noparam // // @return Code de retour: // @retval none // // // @nothrow //------------------------------------------------------------------------------------ int LCCV_ModulatedDampedMode( float* aFrequency ) { int retCode; float m1CurrentGain; int i,j; unsigned int time; unsigned int maxTime; float frequency; float amplitude; int isSaturated; float maxAmplitude; float maxFrequency; unsigned int coilToUse; int progressBar; char buffer1[LCD_CHAR_BY_LINE + 1]; char buffer2[LCD_CHAR_BY_LINE + 1]; #ifdef _TRACE_MODE_3 _DEBUG_INIT() sprintf(buffer1, "start" ); // _DEBUG_TRACE( strlen(buffer1), buffer1 ) #endif // Calibration of the analog conditionner retCode = LCCV_SetCalibration(); if( retCode != RC_OK ) return RC_FAILED; // Clear the screen except the title IHM_LCD_ClearDataLines(); // Check the coils presence retCode = lccv_CheckCoilsPresence( &coilToUse ); if( retCode != RC_OK ) return retCode; // Configure the SSR states if( _SettingsMode3 == OPERATION_WITH_2_COILS ) HAL_IO_SetSsrModulatedBothCoils(); else if( coilToUse == 1 ) HAL_IO_SetSsrModulatedCoil1(); else HAL_IO_SetSsrModulatedCoil2(); // Configure the gain of the M1 amplifier // -> set a high gain for M1 amplifier retCode = CAL_SetM1GainValue( MODE3_PHASE1_GAIN_VALUE, &m1CurrentGain ); // Define the starting frequencies float centerFrequency; float driftFrequency; centerFrequency = MODE3_FREQUENCY_START; driftFrequency = centerFrequency * MODE3_FREQUENCY_DRIFT_FACTOR; // Define the number of loops for a whole frequency sweep float nextFrequency = centerFrequency; int loopNb = 0; do { nextFrequency *= MODE3_PHASE1_FREQUENCY_SWEEP_FACTOR; loopNb++; } while( nextFrequency < MODE3_FREQUENCY_SWEEP_MAX ); // Set Fo frequency of the HP & LP filter M2_FILTER_ENABLE(); HAL_TMR_StartHighPassFilter( centerFrequency - driftFrequency ); HAL_TMR_StartLowPassFilter( centerFrequency + driftFrequency ); PULSE_BT_ENABLE(); HAL_TMR_Delay_us( SSR_SWITCH_TIME_us ); // Set BT pulse to LOW PULSE_BT_LOW(); HAL_TMR_Delay_ms( 100 ); i=0; maxAmplitude = 0; maxFrequency = 0; progressBar = 0; if( _CurrentLanguage == LANGUAGE_FRENCH ) { strcpy( buffer1, " Balayage des "); strcpy( buffer2, " fréquences "); } else { strcpy( buffer1, " Frequencies "); strcpy( buffer2, " sweep "); } ihm_LCD_DisplayStringFromAddress( LCD_LINE_2_ADDRESS, strlen(buffer1), buffer1); ihm_LCD_DisplayStringFromAddress( LCD_LINE_3_ADDRESS, strlen(buffer2), buffer2); // Display the progress bar IHM_LCD_DisplayProgressBar( progressBar ); // Acquisition parameters: 10ms at 25kSps // -> no precision _SampleRate = ADC_RATE_25KSPS; _AcquisitionDuration = 10; // ---------------------------------------------------------------- // PHASE 1 : Frequency sweep // Coarse-step sweep // Set initial half-period in us time = (unsigned int)(1000000 / MODE3_FREQUENCY_START / 2); do { // Disable Timer 1 : atomic operation TIMER1_INTERRUPT_DISABLE(); PULSE_BT_ENABLE(); HAL_TMR_Delay_us( SSR_SWITCH_TIME_us ); // Damp the previous wire oscillation PULSE_BT_HIGH(); HAL_TMR_Delay_ms( 10 ); // Send a set of pulses for( j=0; j MODE3_PHASE1_MIN_AMPLITUDE ) { // This amplitude is the highest obtained if( amplitude > maxAmplitude ) { // Save the maximum amplitude maxAmplitude = amplitude; // Save the found fraquency maxFrequency = frequency; // Save the stimulus half period maxTime = time; #ifdef _TRACE_MODE_3 sprintf(buffer1, "f: %.1f", frequency ); _DEBUG_TRACE( strlen(buffer1), buffer1 ) sprintf(buffer1, "a: %.3f", amplitude ); _DEBUG_TRACE( strlen(buffer1), buffer1 ) sprintf(buffer1, "time: %d", time ); _DEBUG_TRACE( strlen(buffer1), buffer1 ) #endif } } // Continue the frequency sweep i++; // Update the center frequency of the M2 filter centerFrequency *= MODE3_PHASE1_FREQUENCY_SWEEP_FACTOR; driftFrequency = centerFrequency * MODE3_FREQUENCY_DRIFT_FACTOR; HAL_TMR_ChangeLowPassFilter( centerFrequency + driftFrequency ); HAL_TMR_ChangeHighPassFilter( centerFrequency - driftFrequency ); // Set the slot time (corresponding to 1/2 period) time = (unsigned int)(1000000 / centerFrequency / 2); // Display the progress bar if( (i * LCD_PROGRESS_BAR_STEPS / loopNb ) > progressBar ) { progressBar = (i * LCD_PROGRESS_BAR_STEPS / loopNb); IHM_LCD_DisplayProgressBar( progressBar ); } } while( i < (unsigned int)loopNb ); #ifdef _TRACE_MODE_3 sprintf( buffer1, " A: %.3fV F: %.1f", maxAmplitude, maxFrequency ); ihm_LCD_DisplayStringFromAddress( LCD_LINE_4_ADDRESS, strlen(buffer1), buffer1); #endif // No oscillation found if( maxFrequency == 0 ) { // Measurment termination lccv_EndOfOperation(); return RC_NO_OSCILLATION; } // ---------------------------------------------------------------- // PHASE 2 : Frequency measurement // Fine-step sweep // Acquisition parameters: 20ms at 50kSps // -> fine precision _SampleRate = ADC_RATE_50KSPS; _AcquisitionDuration = 30; // Center filter on the found max frequency frequency = maxFrequency * (1 + MODE3_PHASE2_FREQUENCY_DRIFT_FACTOR); HAL_TMR_ChangeLowPassFilter( (int)frequency ); frequency = maxFrequency * (1 - MODE3_PHASE2_FREQUENCY_DRIFT_FACTOR); HAL_TMR_ChangeHighPassFilter( (int)frequency ); HAL_TMR_Delay_ms( 1 ); // Define the number of loops for the fine frequency sweep time = (1000000 / maxFrequency / 2); time *= (MODE3_PHASE1_FREQUENCY_SWEEP_FACTOR - 1); loopNb = time / MODE3_PHASE2_PERIOD_SWEEP_FINE_STEP ; // Start stimulation with a fine step sweep : // - from a half coarse-step before the found frequency // - to a half coarse-step after the found frequency i= loopNb / 2; do { // Disable Timer 1 : atomic operation TIMER1_INTERRUPT_DISABLE(); PULSE_BT_ENABLE(); HAL_TMR_Delay_us( SSR_SWITCH_TIME_us ); // Damp the previous wire oscillation PULSE_BT_HIGH(); HAL_TMR_Delay_ms( 10 ); // Send a set of pulses for( j=0; j -(loopNb / 2) ); // Enable Timer 1 TIMER1_INTERRUPT_ENABLE(); // Result displaying if( amplitude > MODE3_PHASE2_MIN_AMPLITUDE ) { // Save the frequency *aFrequency = frequency; if( _SpeakerState == SPEAKER_ON ) { SPEAKER_ENABLE(); HAL_TMR_Delay_ms( 750 ); HAL_TMR_Delay_ms( 750 ); } } else { // Measurment termination lccv_EndOfOperation(); return RC_INSUFFICIENT_AMPLITUDE; } // Measurment termination lccv_EndOfOperation(); return RC_OK; } //------------------------------------------------------------------------------------ // LCCV_ImpulsiveDampedMode() //------------------------------------------------------------------------------------ // @brief Measurement mode with high voltage impulsion stimulation // see LCCV specification : MODE 2 // // @noparam // // @return Code de retour: // @retval none // // // @nothrow //------------------------------------------------------------------------------------ int LCCV_ImpulsiveDampedMode( float* aFrequency ) { int retCode; int i, j; float m1CurrentGain; float frequency; float meanFrequency; float amplitude; int isSaturated; unsigned int coilToUse; // Calibration of the analog conditionner retCode = LCCV_SetCalibration(); if( retCode != RC_OK ) return RC_FAILED; // Clear the screen except the title IHM_LCD_ClearDataLines(); // Check the coils presence retCode = lccv_CheckCoilsPresence( &coilToUse ); if( retCode != RC_OK ) return retCode; // Configure the SSR states if( _SettingsMode2 == OPERATION_WITH_2_COILS ) HAL_IO_SetSsrImpulsiveBothCoils(); else if( coilToUse == 1 ) HAL_IO_SetSsrImpulsiveCoil1(); else HAL_IO_SetSsrImpulsiveCoil2(); // Define the starting frequencies float centerFrequency; float driftFrequency; centerFrequency = MODE2_FREQUENCY_START; driftFrequency = centerFrequency * MODE2_FREQUENCY_DRIFT_FACTOR; // Define the number of loops for a whole frequency sweep float nextFrequency = centerFrequency; int loopNb = 0; do { nextFrequency *= MODE2_FREQUENCY_SWEEP_FACTOR; loopNb++; } while( nextFrequency < MODE2_FREQUENCY_SWEEP_MAX ); // Enable M2 filter M2_FILTER_ENABLE(); // Set Fo frequency of the low pass filter HAL_TMR_StartLowPassFilter( centerFrequency + driftFrequency ); HAL_TMR_Delay_ms( 5 ); // Configure the gain of the M1 amplifier // -> set a high gain for M1 amplifier if( _SettingsMode2 == OPERATION_WITH_1_COIL ) retCode = CAL_SetM1GainValue( MODE2_GAIN_VALUE_1_COIL, &m1CurrentGain ); else retCode = CAL_SetM1GainValue( MODE2_GAIN_VALUE_2_COILS, &m1CurrentGain ); #ifdef _TRACE_MODE_2 char buffer1[20]; float voltage; _DEBUG_INIT() LCCV_GetHighPulseVoltage( &voltage ); sprintf(buffer1, "Initial VHT: %.1f", voltage ); _DEBUG_TRACE( strlen(buffer1), buffer1 ) #endif // Disable Timer 1 : atomic operation TIMER1_INTERRUPT_DISABLE(); // Start HV generation HAL_TMR_StartHighVoltageCommand(); HAL_TMR_Delay_ms( 20 ); // Send HV pulse PULSE_HT_ENABLE(); HAL_TMR_Delay_us( 750 ); HAL_TMR_Delay_us( 750 ); PULSE_HT_DISABLE(); // Stop HV generation HAL_TMR_StopHighVoltageCommand(); // Enable Timer 1 TIMER1_INTERRUPT_ENABLE(); // Connect the coil for measurement (if necessary) if( _SettingsMode2 == OPERATION_WITH_1_COIL ) { // Wait before measure enable HAL_TMR_Delay_ms( 5 ); if( coilToUse == 1 ) COIL_1_MEASURE_ENABLE(); else COIL_2_MEASURE_ENABLE(); } // Set Fo frequency of the HP filter ( !! use Timer 3 as HV generation !! ) HAL_TMR_StartHighPassFilter( centerFrequency - driftFrequency ); // HV response stabilisation HAL_TMR_Delay_ms( 5 ); // ------------------------------------------------------------------------- // Center the filter frequency // Acquisition parameters: 5ms at 25kSps // -> no frequency precision needed _SampleRate = ADC_RATE_25KSPS; _AcquisitionDuration = 5; j=0; do { // Get the AC signal amplitude LCCV_GetM2OutputFrequency( &frequency, &litude, &isSaturated ); // Disable Timer 1 : atomic operation TIMER1_INTERRUPT_DISABLE(); // Minimum required intial amplitude if( amplitude > MODE2_INITAL_AMPLITUDE ) { // Change center frequency centerFrequency = frequency; // Enable speaker if( _SpeakerState == SPEAKER_ON ) SPEAKER_ENABLE(); // Terminate the frequency centering j = loopNb; } else { // Change Fo frequency centerFrequency *= MODE2_FREQUENCY_SWEEP_FACTOR; } driftFrequency = centerFrequency * MODE2_FREQUENCY_DRIFT_FACTOR; // Update filter frequencies HAL_TMR_ChangeLowPassFilter( centerFrequency + driftFrequency ); HAL_TMR_ChangeHighPassFilter( centerFrequency - driftFrequency ); HAL_TMR_Delay_ms( 1 ); // Enable Timer 1 TIMER1_INTERRUPT_ENABLE(); j++; } while( j < loopNb ); // ------------------------------------------------------------------------- // Signal acquisition // Acquisition parameters: 30ms at 50kSps // -> cute frequency precision (increase the number of acquired periods) _SampleRate = ADC_RATE_50KSPS; _AcquisitionDuration = 30; if( j == loopNb + 1) { meanFrequency = 0; // Moving average computation for( i=0; i no precision needed during stabilisation _SampleRate = ADC_RATE_25KSPS; _AcquisitionDuration = 10; // PID factors _Kp = 0.4; _Kd = 0.8; _Ki = 0.05; // Initialise and stabilise the oscillation retCode = lccv_InitialiseSustainedOscillation( &m1TargetGain, &frequencyMean, &frequencyRange ); // Clear the screen IHM_LCD_ClearDataLines(); // --------------------------------------------------------------- // Phase 2 // The wire is oscillating if( retCode == RC_OK ) { // Recenter frequency // retCode = lccv_RecenterFrequency( &m1TargetGain, frequencyMean, ¢erFrequency ); float referenceFrequency = frequencyMean; float measureDrift; // Speaker activation if( _SpeakerState == SPEAKER_ON ) SPEAKER_ENABLE(); else SPEAKER_DISABLE(); // Initialisation with the last known values for( indexMean = 0; indexMean < MODE4_PHASE2_MA_FREQUENCY_ELEMENTS; indexMean++ ) frequencyTable[indexMean] = frequencyMean; indexMean = 0; gainData.previousDeviation = 0; gainData.targetGain = m1TargetGain; gainData.previousTargetGain = gainData.targetGain; // Acquisition parameters: 40ms at 50kSps // -> cute frequency precision (increase the number of acquired periods) _SampleRate = ADC_RATE_50KSPS; _AcquisitionDuration = 40; gainData.mostRecentDeviationIdx = 0; gainData.maxGain = gainData.targetGain; // Phase 2 : Sustain the coil oscillation while( _StoppingOperation == LCCV_NO ) { // Gain controller : try to reach the voltage order lccv_GainController( &gainData ); #ifdef _TRACE_MODE_4 sprintf(buffer1, "F: %.1f Hz", gainData.frequency ); _DEBUG_TRACE( strlen(buffer1), buffer1 ); #endif // For the next loop gainData.previousTargetGain = gainData.targetGain; gainData.previousDeviation = gainData.currentDeviation; m1TargetGain = gainData.targetGain; // Change the M1 gain CAL_SetM1GainValue( m1TargetGain, &m1CurrentGain ); // Save into frequency table frequencyTable[indexMean] = gainData.frequency; if( indexMean < MODE4_PHASE2_MA_FREQUENCY_ELEMENTS-1 ) indexMean++; else indexMean = 0; // Perform the moving average frequencyMean = 0; for( j = 0; j < MODE4_PHASE2_MA_FREQUENCY_ELEMENTS; j++ ) frequencyMean += frequencyTable[j]; frequencyMean /= MODE4_PHASE2_MA_FREQUENCY_ELEMENTS; // Display result IHM_LCD_DisplayFrequencyResult( RC_OK, frequencyMean ); //sprintf( buffer2, " A: %.3f V G: %.1f ", gainData.amplitude, gainData.targetGain ); //ihm_LCD_DisplayStringFromAddress( LCD_LINE_4_ADDRESS, strlen(buffer2), buffer2); // Wait HAL_TMR_Delay_ms( MODE4_PHASE2_MEASURE_INTERVAL ); // Recenter the filter range measureDrift = frequencyMean / referenceFrequency; HAL_TMR_ChangeHighPassFilter( highPassFilterFrequency[frequencyRange] * measureDrift ); HAL_TMR_ChangeLowPassFilter( lowPassFilterFrequency[frequencyRange] * measureDrift ); // End of loop i++; } } else { // Display result IHM_LCD_DisplayFrequencyResult( retCode, 0 ); HAL_TMR_Delay_ms( 1000 ); HAL_TMR_Delay_ms( 1000 ); } // Measurment termination lccv_EndOfOperation(); return RC_OK; } //------------------------------------------------------------------------------------ // lccv_InitialiseSustainedOscillation() //------------------------------------------------------------------------------------ // @brief Try to make the coil enter into auto oscillation // -> stabilize and maintain the voltage order // -> the filter shall be previously set // @noparam // // @return Code de retour: // @retval none // // // @nothrow //------------------------------------------------------------------------------------ int lccv_InitialiseSustainedOscillation( float* aCurrentGain, float* aFrequency, int* aFrequencyRange ) { int retCode; int i, j; int isStarted = LCCV_NO; int isStabilised = LCCV_NO; float m1CurrentGain; float m1TargetGain; float deviationTable[ MODE4_PHASE1_STABILISATION_NB ]; float frequencyTable[ MODE4_PHASE2_MA_FREQUENCY_ELEMENTS ]; float frequencyMean; int indexMean = 0; int progressBar = 0; ST_GAIN_CONTROL gainData; char buffer[LCD_CHAR_BY_LINE + 1]; int frequencyRange = 0; int ctr; // Update Fo frequency of the high pass filter HAL_TMR_ChangeHighPassFilter( highPassFilterFrequency[frequencyRange] ); // Update Fo frequency of the low pass filter HAL_TMR_ChangeLowPassFilter( lowPassFilterFrequency[frequencyRange] ); HAL_TMR_Delay_ms( 10 ); // Set initial gain retCode = CAL_SetM1GainValue( MODE4_PHASE1_MAX_GAIN, &m1CurrentGain ); // Send a stimulation LCCV_ModulatedStimulationForSustainedMode( frequencyRange ); HAL_TMR_Delay_ms( 10 ); // Initialise the target gain m1TargetGain = m1CurrentGain; gainData.targetGain = m1TargetGain; gainData.mostRecentDeviationIdx = 0; gainData.maxGain = gainData.targetGain; gainData.previousTargetGain = gainData.targetGain; gainData.previousDeviation = 0; i = 0; ctr = 0; while( frequencyRange < MODE4_FREQUENCY_RANGE_NB ) { // Gain controller : try to reach the voltage order lccv_GainController( &gainData ); // Save into deviation table to determine the oscillation stabilisation deviationTable[i++] = gainData.currentDeviation; if( i == MODE4_PHASE1_STABILISATION_NB ) i = 0; // For the next loop m1TargetGain = gainData.targetGain; // Oscillation has started if( gainData.amplitude > MODE4_PHASE1_MIN_AMPLITUDE ) isStarted = LCCV_YES; if( isStarted == LCCV_YES ) { // Check for oscillation stabilisation for( j=0; j MODE4_PHASE1_DEVIATION_STABILISATION || deviationTable[j] < -MODE4_PHASE1_DEVIATION_STABILISATION ) break; } if( j == MODE4_PHASE1_STABILISATION_NB ) isStabilised = LCCV_YES; // Oscillation is stabilised if( isStabilised == LCCV_YES ) { // Save the current target gain *aCurrentGain = m1TargetGain; // Perform the moving average on the last known frequency values frequencyMean = 0; for( j = 0; j < MODE4_PHASE2_MA_FREQUENCY_ELEMENTS; j++ ) frequencyMean += frequencyTable[j]; frequencyMean /= MODE4_PHASE2_MA_FREQUENCY_ELEMENTS; // Save the last known frequency *aFrequency = frequencyMean; // Save the filter frequency range *aFrequencyRange = frequencyRange; // Exit loop return RC_OK; } } // Change the M1 gain CAL_SetM1GainValue( m1TargetGain, &m1CurrentGain ); // Save into frequency table frequencyTable[indexMean] = gainData.frequency; if( indexMean < MODE4_PHASE2_MA_FREQUENCY_ELEMENTS-1 ) indexMean++; else indexMean = 0; // Wait time /* switch( frequencyRange ) { case 0 : // Surface sensor : not tighted HAL_TMR_Delay_ms( 30 ); break; case 1 : // Surface sensor : middle range HAL_TMR_Delay_ms( 10 ); break; case 2 : // Surface sensor : upper range HAL_TMR_Delay_ms( 5 ); break; case 3 : // Generic sensor : SG1 //HAL_TMR_Delay_ms( 5 ); break; case 4 : // Generic sensor : production //HAL_TMR_Delay_ms( 1 ); break; default : return RC_FAILED; } */ // Oscillation not started on time // Oscillation started but not stabilised on time if( (ctr > MODE4_PHASE1_MIN_TURN && isStarted == LCCV_NO) || (ctr > MODE4_PHASE1_MAX_MEASURE_NB && isStarted == LCCV_YES) ) { isStarted = LCCV_NO; ctr = 0; frequencyRange++; // Update Fo frequency of the high pass filter HAL_TMR_ChangeHighPassFilter( highPassFilterFrequency[frequencyRange] ); // Update Fo frequency of the low pass filter HAL_TMR_ChangeLowPassFilter( lowPassFilterFrequency[frequencyRange] ); HAL_TMR_Delay_ms( 10 ); // Set initial gain retCode = CAL_SetM1GainValue( MODE4_PHASE1_MAX_GAIN, &m1CurrentGain ); // Send a stimulation LCCV_ModulatedStimulationForSustainedMode( frequencyRange ); HAL_TMR_Delay_ms( 10 ); // Display the progress bar if( (frequencyRange * LCD_PROGRESS_BAR_STEPS / MODE4_FREQUENCY_RANGE_NB) > progressBar ) { progressBar = (frequencyRange * LCD_PROGRESS_BAR_STEPS / MODE4_FREQUENCY_RANGE_NB); IHM_LCD_DisplayProgressBar( progressBar ); } } ctr++; //sprintf( buffer, " A: %.3f V G: %.1f ", gainData.amplitude, gainData.targetGain ); //ihm_LCD_DisplayStringFromAddress( LCD_LINE_4_ADDRESS, strlen(buffer), buffer); } return RC_NO_OSCILLATION; } //------------------------------------------------------------------------------------ // lccv_RecenterFrequency() //------------------------------------------------------------------------------------ // @brief Try to center the filter frequency on the oscillation frequency // // @noparam // // @return Code de retour: // @retval none // // // @nothrow //------------------------------------------------------------------------------------ int lccv_RecenterFrequency( float* aM1TargetGain, float aFrequencyMean, float* aCenterFrequency ) { int retCode; int i, j; float centerFrequency; float driftFrequency; float frequencyGap; int isStarted = LCCV_NO; int isStabilised = LCCV_NO; float m1CurrentGain; float m1TargetGain; float gainTable[ 10 ]; int gainIndex; float maGainTable[ 5 ]; float frequencyTable[ 5 ]; int maGainIndex; float maGainValue; float lowestGainValue; float lowestGainFilterFrequency; float regressionSlope; float frequencyMean; int indexMean = 0; int progressBar = 0; int loopNb; int nbFrequencyChange = 0; ST_GAIN_CONTROL gainData; char buffer[LCD_CHAR_BY_LINE + 1]; /* TODO ... TODO ... TODO ... TODO ... Do an offset on the high pass frequency (instead of center frequency) Find the best gain value Do the same with the low pass value */ // Initial center frequency centerFrequency = *aCenterFrequency; driftFrequency = centerFrequency * MODE4_FREQUENCY_DRIFT_FACTOR; // Update Fo frequency of the high pass filter HAL_TMR_ChangeHighPassFilter( centerFrequency - driftFrequency ); // Update Fo frequency of the low pass filter HAL_TMR_ChangeLowPassFilter( centerFrequency + driftFrequency ); // Set initial gain retCode = CAL_SetM1GainValue( *aM1TargetGain, &m1CurrentGain ); // Initialise the target gain m1TargetGain = m1CurrentGain; gainData.targetGain = m1TargetGain; gainData.mostRecentDeviationIdx = 0; gainData.maxGain = gainData.targetGain; gainData.previousTargetGain = gainData.targetGain; gainData.previousDeviation = 0; for( i=0; i<10; i++ ) gainTable[i] = m1TargetGain; for( i=0; i<5; i++ ) maGainTable[i] = m1TargetGain; for( i=0; i<5; i++ ) frequencyTable[i] = centerFrequency; lowestGainValue = m1TargetGain; lowestGainFilterFrequency = *aCenterFrequency; gainIndex = 0; maGainIndex = 0; j = 0; int degressiveSweep; if( gainData.frequency < centerFrequency ) degressiveSweep = 1; else degressiveSweep = 0; do { // // Gain controller lccv_GainController( &gainData ); // For the next loop gainData.previousTargetGain = gainData.targetGain; gainData.previousDeviation = gainData.currentDeviation; m1TargetGain = gainData.targetGain; gainTable[gainIndex++] = m1TargetGain; if( gainIndex > 10 - 1 ) gainIndex = 0; // Change the M1 gain CAL_SetM1GainValue( m1TargetGain, &m1CurrentGain ); frequencyGap = gainData.frequency - centerFrequency; if( j > 10 ) { // Compute the moving average for gains maGainValue = 0; for( i=0; i<10; i++ ) maGainValue += gainTable[i]; maGainValue /= 10; maGainTable[maGainIndex] = maGainValue; // Keep the current center frequency frequencyTable[maGainIndex++] = centerFrequency; if( maGainIndex > 5 - 1 ) maGainIndex = 0; LCCV_RegressionLineSlope( 5, frequencyTable, maGainTable, ®ressionSlope ); #ifdef _TRACE_MODE_4 sprintf(buffer, "G0: %.1f %.1f", maGainTable[0], frequencyTable[0] ); // _DEBUG_TRACE( strlen(buffer), buffer ) sprintf(buffer, "G1: %.1f %.1f", maGainTable[1], frequencyTable[1] ); // _DEBUG_TRACE( strlen(buffer), buffer ) sprintf(buffer, "G2: %.1f %.1f", maGainTable[2], frequencyTable[2] ); // _DEBUG_TRACE( strlen(buffer), buffer ) sprintf(buffer, "G3: %.1f %.1f", maGainTable[3], frequencyTable[3] ); // _DEBUG_TRACE( strlen(buffer), buffer ) sprintf(buffer, "G4: %.1f %.1f", maGainTable[4], frequencyTable[4] ); // _DEBUG_TRACE( strlen(buffer), buffer ) sprintf(buffer, "Slope %d: %.1f", nbFrequencyChange, regressionSlope ); // _DEBUG_TRACE( strlen(buffer), buffer ) #endif // Change frequency sweep direction // The gain are increasing if( degressiveSweep == 1 && regressionSlope < 0 ) degressiveSweep = 0; if( degressiveSweep == 0 && regressionSlope > 0 ) degressiveSweep = 1; // Gain stabilisation if( nbFrequencyChange > 5 && regressionSlope < 0.5 && regressionSlope > -0.5) { break; } // Filter centered on the fundamental frequency if( frequencyGap < 10 && frequencyGap > -10 ) { break; } // Change center frequency if( degressiveSweep == 1 ) centerFrequency -= 10; else centerFrequency += 10; /* if( frequencyGap >= 0 ) { if( frequencyGap * 0.03 < 5 ) centerFrequency += 5; else centerFrequency += (frequencyGap * 0.03); } else { if( frequencyGap * 0.03 > -5 ) centerFrequency -= 5; else centerFrequency += (frequencyGap * 0.03); }*/ driftFrequency = centerFrequency * MODE4_FREQUENCY_DRIFT_FACTOR; // Update Fo frequency of the high pass filter HAL_TMR_ChangeHighPassFilter( centerFrequency - driftFrequency ); // Update Fo frequency of the low pass filter HAL_TMR_ChangeLowPassFilter( centerFrequency + driftFrequency ); j = 0; nbFrequencyChange++; } else { j++; } /* sprintf( buffer, " RECENTER " ); ihm_LCD_DisplayStringFromAddress( LCD_LINE_1_ADDRESS, strlen(buffer), buffer); sprintf( buffer, " F: %.1f ", centerFrequency ); ihm_LCD_DisplayStringFromAddress( LCD_LINE_2_ADDRESS, strlen(buffer), buffer); sprintf( buffer, " Sl: %.2f ", regressionSlope ); ihm_LCD_DisplayStringFromAddress( LCD_LINE_3_ADDRESS, strlen(buffer), buffer); sprintf( buffer, " A: %.3f V G: %.1f ", gainData.amplitude, gainData.targetGain ); ihm_LCD_DisplayStringFromAddress( LCD_LINE_4_ADDRESS, strlen(buffer), buffer); */ } while( nbFrequencyChange < 100 ); *aCenterFrequency = centerFrequency; *aM1TargetGain = m1TargetGain; return RC_OK; } //------------------------------------------------------------------------------------ // lccv_GainController() //------------------------------------------------------------------------------------ // @brief Control the gain correction so as to reach the voltage order // // @noparam // // @return Code de retour: // @retval none // // // @nothrow //------------------------------------------------------------------------------------ int lccv_GainController( ST_GAIN_CONTROL* aGainData ) { float frequency; float amplitude; int isSaturated; float deviation; float integralDeviation; float derivativeDeviation; char buffer[LCD_CHAR_BY_LINE]; // Get the AC signal amplitude LCCV_GetM2OutputFrequency( &frequency, &litude, &isSaturated ); // Porportionnal term // -> get the deviation between the target amplitude and the current oscillation amplitude (in mV) deviation = LOOPBACK_TARGET_AMPLITUDE - amplitude; // Update the deviation index : next writing index if( aGainData->mostRecentDeviationIdx < MODE4_PHASE2_DEV_SAVINGS-1) (aGainData->mostRecentDeviationIdx)++; else aGainData->mostRecentDeviationIdx = 0; // Keep the previous deviations (MODE4_PHASE2_DEV_SAVINGS elements saved) aGainData->deviation[ aGainData->mostRecentDeviationIdx ] = deviation; // Integral term // -> sum of previous errors integralDeviation = deviation + aGainData->previousDeviation; // Derivative term // -> compute the slope of the deviation derivativeDeviation = deviation - aGainData->previousDeviation; // Update the gain aGainData->targetGain *= (1 + integralDeviation * _Ki + deviation * _Kp + derivativeDeviation * _Kd); // Input oscillation is not saturated if( isSaturated == LCCV_NO ) { // Gain rising slope limitation if( aGainData->previousTargetGain * (1 + MODE4_PHASE1_MAX_GAIN_SLOPE_DRIFT ) < aGainData->targetGain ) aGainData->targetGain = aGainData->previousTargetGain * (1 + MODE4_PHASE1_MAX_GAIN_SLOPE_DRIFT ); // Gain falling slope limitation else if( aGainData->previousTargetGain * (1 - MODE4_PHASE1_MAX_GAIN_SLOPE_DRIFT ) > aGainData->targetGain ) aGainData->targetGain = aGainData->previousTargetGain * (1 - MODE4_PHASE1_MAX_GAIN_SLOPE_DRIFT ); //sprintf( buffer, " NO " ); //ihm_LCD_DisplayStringFromAddress( LCD_LINE_1_ADDRESS, strlen(buffer), buffer); } // Input oscillation is saturated else { // Force a gain reduction aGainData->targetGain *= (1 - MODE4_SATURATION_GAIN_REDUCTION); //sprintf( buffer, "YES " ); //ihm_LCD_DisplayStringFromAddress( LCD_LINE_1_ADDRESS, strlen(buffer), buffer); } aGainData->previousTargetGain = aGainData->targetGain; // Gain consistency check if( aGainData->targetGain > MODE4_PHASE1_MAX_GAIN ) aGainData->targetGain = MODE4_PHASE1_MAX_GAIN; else if( aGainData->targetGain < MODE4_PHASE1_MIN_GAIN ) aGainData->targetGain = MODE4_PHASE1_MIN_GAIN; // Save the current deviation aGainData->currentDeviation = deviation; aGainData->previousDeviation = deviation; aGainData->amplitude = amplitude; aGainData->frequency = frequency; #ifdef _TRACE_MODE_4 sprintf(buffer, " G: %d ", (int)aGainData->targetGain ); // _DEBUG_TRACE( strlen(buffer), buffer ); sprintf(buffer, " A: %.3fV", amplitude ); // _DEBUG_TRACE( strlen(buffer), buffer ); #endif return RC_OK; } //------------------------------------------------------------------------------------ // lccv_CheckCoilsPresence() //------------------------------------------------------------------------------------ // @brief Check the number of present coils // // @noparam // // @return Code de retour: // @retval none // // // @nothrow //------------------------------------------------------------------------------------ int lccv_CheckCoilsPresence( unsigned int* aCoilToUse ) { int retCode; float coil1Resistance; float coil2Resistance; unsigned int coilNumber; // Check the coils presence retCode = LCCV_CoilsResistanceMeasure( &coil1Resistance, &coil2Resistance ); if( retCode != RC_OK ) return RC_FAILED; // Update the number of coils coilNumber = 2; *aCoilToUse = 1; // Coil 1 is not operationnal if( coil1Resistance == COIL_RESISTOR_OPEN_CIRCUIT || coil1Resistance == COIL_RESISTOR_SHORT_CIRCUIT ) { coilNumber--; *aCoilToUse = 2; } // Coil 2 is not operationnal if( coil2Resistance == COIL_RESISTOR_OPEN_CIRCUIT || coil2Resistance == COIL_RESISTOR_SHORT_CIRCUIT ) { coilNumber--; *aCoilToUse = 1; } // No coil detected if( coilNumber == 0 ) return RC_NO_COIL; // Mode 2 with 2 coils if( _CurrentOperation == OPERATION_MODE_2 && _SettingsMode2 == OPERATION_WITH_2_COILS ) { if( coilNumber == 2 ) return RC_OK; else return RC_MISSING_COIL; } // Mode 2 with 1 coil if( _CurrentOperation == OPERATION_MODE_2 && _SettingsMode2 == OPERATION_WITH_1_COIL ) { if( coilNumber == 2 || coilNumber == 1 ) return RC_OK; else return RC_MISSING_COIL; } // Mode 3 with 2 coils if( _CurrentOperation == OPERATION_MODE_3 && _SettingsMode3 == OPERATION_WITH_2_COILS ) { if( coilNumber == 2 ) return RC_OK; else return RC_MISSING_COIL; } // Mode 3 with 1 coil if( _CurrentOperation == OPERATION_MODE_3 && _SettingsMode3 == OPERATION_WITH_1_COIL ) { if( coilNumber == 2 || coilNumber == 1 ) return RC_OK; else return RC_MISSING_COIL; } // Mode 4 with 2 coils if( _CurrentOperation == OPERATION_MODE_4 ) { if( coilNumber == 2 ) return RC_OK; else return RC_MISSING_COIL; } return RC_OK; } //------------------------------------------------------------------------------------ // lccv_EndOfOperation() //------------------------------------------------------------------------------------ // @brief Terminate the measurment, keep the LCCV in idle state // // @noparam // // @return Code de retour: // @retval none // // // @nothrow //------------------------------------------------------------------------------------ int lccv_EndOfOperation( void ) { float dummy; // Speaker deactivation SPEAKER_DISABLE(); // Set gain to the minimum CAL_SetM1GainValue( M1_MIN_LIMIT_GAIN, &dummy ); // Filter deactivation M2_FILTER_DISABLE(); HAL_TMR_StopLowPassFilter(); HAL_TMR_StopHighPassFilter(); // Disable all SSR of tha analog conditionner DISABLE_ALL_SSR(); return RC_OK; }