/************************************************************************* $Archive: /PACS/OnBoard/ChopCtrl.c $ $Revision: 1.18 $ $Date: 2009/04/23 13:51:12 $ $Author: amazy $ $Log: ChopCtrl.c,v $ Revision 1.18 2009/04/23 13:51:12 amazy 6.029 * * 14 30/09/08 15:19 Amazy * - increased grating power limit to 150mA and made it commandable * - added a synchro counter in HK * - new parameters value for DMC_SYNCHRONIZE_ON_DET to avoid using the * synchro to trigger the mechanisms move * - In photo, the RedSpuTransmission mode was not copied into red science * headers. It is now corrected * * 13 28/07/08 15:50 Amazy * added QM chopper field plate LUT * * 12 6/04/08 11:45p Amazy * - added a filter to the grating controller * - now resetting ChopperOut in the chopper enable * * 11 6/28/07 4:33p Amazy * v6.020 * * 10 6/09/07 2:47p Amazy * v6.019 chopper filter divisor changed from 1000 to 1000000 * * 9 6/05/07 5:16p Amazy * v6.018, FW is now turning 1.5 turn. updated FW default params * * 8 5/31/07 11:27a Amazy * v6.018 * * 7 3/20/07 3:14p Amazy * * 6 2/07/07 11:30a Amazy * - The open loop code for the grating has been updated and validated on * DMC FM with QM grating * - The position limit error is now signaled in the chopper controller * (error code 0x0b15). The position limit * is stored in the field 'scaling' of the chopper pid controller. Its * default value is 0x7FFFFFFF. * * 46 27/03/06 14:38 Amazy * In the case the amplitude of a move of the chopper was 0, DMC was * returning a NACK. It is now returning a PACK * * 45 3/14/06 4:31p Pacs Egse * Version 6.001 * Cleaned and commented *************************************************************************/ #ifdef SIMULATOR #include "virtuosoSim.h" #else // SIMULATOR #include "v_macro.h" #include "node1.h" #include "k_struct.h" #endif #include "params.h" #include "error.h" /**************************************************************** GLOBAL VARIABLES ****************************************************************/ int gChopperNextTarget = 0; int gChopperTarget = 0; int gChopperCurrentSetPoint = 0; int gChopperTrajectoryIndex = 0; int gChopperTrajectoryInc = 0; int gChopperCurrentPosition = 0; int gDitheringDistributionIndex = 0; int gDitheringAmplitude = 16; //Note : there is another global variable initialisation at the end of the file /**************************************************************** FUNCTIONS DECLARATION ****************************************************************/ void InitializeChopperControllerParams(ChopperControllerParams* p_controller); void ComputeFloatChopperControllerParams(ChopperControllerParams* p_controller); void DisableChopperController(); // commands function BOOL CommandMoveChopperAbsolute(CommandParameter param); BOOL CommandMoveChopperRelative(CommandParameter param); BOOL CommandMoveChopperAbsoluteWithDither(CommandParameter param); BOOL CommandMoveChopperRelativeWithDither(CommandParameter param); BOOL CommandSwitchOnChopperController(CommandParameter paramNotUsed); BOOL CommandSwitchOffChopperController(CommandParameter paramNotUsed); BOOL CommandEnableChopperController(CommandParameter paramNotUsed); BOOL CommandDisableChopperController(CommandParameter paramNotUsed); /**************************************************************** FUNCTIONS IMPLEMENTATION ****************************************************************/ void InitializeChopperControllerParams(ChopperControllerParams* p_controller) { int i=0; float x; float y; InitializePidControllerParams(&(p_controller->PidController)); p_controller->PidController.Kp = 0x000683F8; p_controller->PidController.Ki = 0x018392C0; p_controller->PidController.Kd = 0x00000032; p_controller->PidController.Kf = 0x00000898; p_controller->PidController.Rate = 0x0000024A; p_controller->PidController.AccumulatorLimit = 0x3FFFFFFF; p_controller->PidController.OutputLimit = 0x000069DC; p_controller->PidController.Scaling = 0x000061A8; //for chopper, the scaling is actually the position limit p_controller->PidController.ErrorLimit = 0x7FFFFFFF; p_controller->PidController.PosOffset = 0xFFFFFB55; p_controller->KiCurr = 0x000468E8; p_controller->SelectFieldPlateLUT = 0; //0 = nominal, 1 = redundant, other = none p_controller->IntFilterN1 = 0x00018AAF; p_controller->IntFilterN2 = 0x00029F4D; p_controller->IntFilterN3 = 0x00018AAF; p_controller->IntFilterD1 = 0x001C11DF; p_controller->IntFilterD2 = 0x000DBB35; p_controller->IntInductance = 0x00000094; p_controller->IntResistance = 0x0000393A; p_controller->IntControlLoopGain = 0x000003B1; //some parameters need to be converted in float ComputeFloatChopperControllerParams(p_controller); //init the chopper position look-up table //FM nominal LUT : y = -1.975E-15x4 + 3.491E-10x3 + 1.903E-06x2 + 0.9689x - 71.158 for (i=0; i < 0x10000; i++) { x = (float)(i - 0x8000); y = -71.158f + 0.9689f * x; y+= 0.000001903f * x * x; y+= 0.0000000003491f * x * x * x; y+= -0.000000000000001975f * x * x * x * x; p_controller->FmNominalFieldPlateLUT[i] = y; } //FM redundant LUT : y = -7.124E-15x4 + 5.803E-10x3 + 3.194E-06x2 + 0.9729x -111.877 for (i=0; i < 0x10000; i++) { x = (float)(i - 0x8000); y = -111.877f + 0.9729f * x; y+= 0.000003194f * x * x; y+= 0.0000000005803f * x * x * x; y+= -0.000000000000007124f * x * x * x * x; p_controller->FmRedundantFieldPlateLUT[i] = y; } //QM nominal LUT : y = 1.1692e-015x4 + 4.5776e-010x3 -6.9173e-007x2 + 9.0144e-001x -4.0116e+001 for (i=0; i < 0x10000; i++) { x = (float)(i - 0x8000); y = -40.116f + 0.90144f * x; y+= -0.0000006917f * x * x; y+= 0.000000000455776f * x * x * x; y+= 0.0000000000000011692f * x * x * x * x; p_controller->QmNominalFieldPlateLUT[i] = y; } //QM redundant LUT : y = -1.2309e-015x4 + 5.3842e-010x3 + 5.7227e-007x2 + 9.6537e-001x + 1.1025e+002 for (i=0; i < 0x10000; i++) { x = (float)(i - 0x8000); y = 110.25f + 0.96537f * x; y+= 0.00000057227f * x * x; y+= 0.00000000053842f * x * x * x; y+= -0.0000000000000012309f * x * x * x * x; p_controller->QmRedundantFieldPlateLUT[i] = y; } //No LUT for (i=0; i < 0x10000; i++) { p_controller->NoFieldPlateLUT[i] = (float)(i - 0x8000); } } void ComputeFloatChopperControllerParams(ChopperControllerParams* p_controller) { //transform some of the parameters into float now to avoid making it at every control loop iteration gpChopperController->FloatKp = ((float)gpChopperController->PidController.Kp)/1000.0f; gpChopperController->FloatKi = ((float)gpChopperController->PidController.Ki)/1000.0f; gpChopperController->FloatKd = ((float)gpChopperController->PidController.Kd)/1000.0f; gpChopperController->FloatKf = ((float)gpChopperController->PidController.Kf)/1000.0f; gpChopperController->FloatKiCurr = ((float)gpChopperController->KiCurr)/1000.0f; gpChopperController->FloatFilterN1 = ((float)gpChopperController->IntFilterN1)/1000000.0f; gpChopperController->FloatFilterN2 = ((float)gpChopperController->IntFilterN2)/1000000.0f; gpChopperController->FloatFilterN3 = ((float)gpChopperController->IntFilterN3)/1000000.0f; gpChopperController->FloatFilterD1 = ((float)gpChopperController->IntFilterD1)/1000000.0f; gpChopperController->FloatFilterD2 = ((float)gpChopperController->IntFilterD2)/1000000.0f; gpChopperController->FloatInductance = ((float)gpChopperController->IntInductance)/1000.0f; gpChopperController->FloatResistance = ((float)gpChopperController->IntResistance)/1000.0f; gpChopperController->FloatControlLoopGain = ((float)gpChopperController->IntControlLoopGain)/1000.0f; if (gpChopperController->SelectFieldPlateLUT == 0) { gpChopperController->OffsetToSelectedLUT = CH_FM_N_LUT2; } else if (gpChopperController->SelectFieldPlateLUT == 1) { gpChopperController->OffsetToSelectedLUT = CH_FM_R_LUT2; } else if (gpChopperController->SelectFieldPlateLUT == 2) { gpChopperController->OffsetToSelectedLUT = CH_QM_N_LUT2; } else if (gpChopperController->SelectFieldPlateLUT == 3) { gpChopperController->OffsetToSelectedLUT = CH_QM_R_LUT2; } else { gpChopperController->OffsetToSelectedLUT = CH_F_NO_LUT2; } } /* FUNCTION : BOOL StartMovingChopper(int target) ********************************************** AUTHOR : AMazy Initialize a move of the chopper PARAMS: target: the target position */ BOOL StartMovingChopper(int target) { int controlWord; // if the controller is not enabled, return FALSE if (!TEST_ONE_BIT(gpChopperController->PidController.PidStatus, K_BMASK_PID_CTRL_STATUS_PID_ENABLED)) { return FALSE; } // test if previous move is finished if ((gpChopperController->PidController.PidStatus & K_BMASK_PID_CTRL_STATUS_MOVING_STATUS) != 0) { return FALSE; } // compute the amplitude of the move (setpoint - target) gpChopperController->SineTransitionAmplitude = (float)(gpChopperController->PidController.CurrentSetPoint - target); if (gpChopperController->SineTransitionAmplitude == 0.0f) { return TRUE; } // compute the initial position in the sine array and the increment to walk through the array gpChopperController->SineTransitionPosInArrayCurrent = (float)(NB_STEPS_IN_PHASE/4); gpChopperController->SineTransitionPosInArrayIncr = ((float)(NB_STEPS_IN_PHASE/2))/(gpChopperController->SineTransitionAmplitude/((float)(gpChopperController->PidController.Rate))); if (gpChopperController->SineTransitionPosInArrayIncr < 0.0f) { gpChopperController->SineTransitionPosInArrayIncr = -gpChopperController->SineTransitionPosInArrayIncr; } gpChopperController->SineTransitionStartSetPoint = (float)(gpChopperController->PidController.CurrentSetPoint); // set the target gpChopperController->PidController.Target = target; if (target > gpChopperController->PidController.CurrentSetPoint) { controlWord = K_BMASK_PID_CTRL_COMMAND_MOVE_UP; } else { controlWord = K_BMASK_PID_CTRL_COMMAND_MOVE_DOWN; } if (gpMim->MechanismsUseSynchro) { controlWord += K_BMASK_PID_CTRL_COMMAND_USE_SYNCHRO; } gpChopperController->PidController.ControlWord = controlWord; return TRUE; } /* CMD_FUNCTION : BOOL CommandMoveChopperAbsolute(CommandParameter param) ********************************************************************** AUTHOR : Amazy USE : Start moving the chopper to a new absolute position PARAMS : Uint : new absolute position */ BOOL CommandMoveChopperAbsolute(CommandParameter param) { if (!StartMovingChopper(param.Int)) { SequencerSendNack(0, COULD_NOT_EXECUTE_COMMAND); SetSequencerError(ERR_SEQUENCER_COULD_NOT_EXECUTE_COMMAND); return FALSE; } SequencerSendAck(DMC_MOVE_CHOP_ABS); return TRUE; }; /* CMD_FUNCTION : BOOL CommandMoveChopperRelative(CommandParameter param) ********************************************************************** AUTHOR : Amazy USE : Start moving the chopper to a new relative position PARAMS : Uint : relative position increment */ BOOL CommandMoveChopperRelative(CommandParameter param) { if (!StartMovingChopper(gpChopperPidController->Target + param.Int)) { SequencerSendNack(0, COULD_NOT_EXECUTE_COMMAND); SetSequencerError(ERR_SEQUENCER_COULD_NOT_EXECUTE_COMMAND); return FALSE; } SequencerSendAck(DMC_MOVE_CHOP_REL); return TRUE; }; /* CMD_FUNCTION : BOOL CommandMoveChopperAbsoluteWithDither(CommandParameter param) ******************************************************************************** AUTHOR : Amazy USE : Start moving the chopper to a new absolute position and add dithering PARAMS : Uint : new absolute position */ BOOL CommandMoveChopperAbsoluteWithDither(CommandParameter param) { int dither = gaDitheringDistribution[gDitheringDistributionIndex]; dither = (gDitheringAmplitude * dither) >> 15; //We assume that DitheringDistribution is made of numbers in [-32768 32767] // increment DitheringDistributionIndex for next time gDitheringDistributionIndex++; gDitheringDistributionIndex &= 0x0000008f; // wrap index at 127 if (!StartMovingChopper(param.Int + dither)) { SequencerSendNack(0, COULD_NOT_EXECUTE_COMMAND); SetSequencerError(ERR_SEQUENCER_COULD_NOT_EXECUTE_COMMAND); return FALSE; } SequencerSendAck(DMC_MOVE_CHOP_ABS_DITHER); return TRUE; }; /* CMD_FUNCTION : BOOL CommandMoveChopperRelativeWithDither(CommandParameter param) ******************************************************************************** AUTHOR : Amazy USE : Start moving the chopper to a new relative position and add dithering PARAMS : Uint : relative position increment */ BOOL CommandMoveChopperRelativeWithDither(CommandParameter param) { int dither = gaDitheringDistribution[gDitheringDistributionIndex]; dither = (gDitheringAmplitude * dither) >> 15; //We assume that DitheringDistribution is made of numbers in [-32768 32767] // increment DitheringDistributionIndex for next time gDitheringDistributionIndex++; gDitheringDistributionIndex &= 0x0000008f; // wrap index at 127 if (!StartMovingChopper(gpChopperPidController->Target + param.Int + dither)) { SequencerSendNack(0, COULD_NOT_EXECUTE_COMMAND); SetSequencerError(ERR_SEQUENCER_COULD_NOT_EXECUTE_COMMAND); return FALSE; } SequencerSendAck(DMC_MOVE_CHOP_REL_DITHER); return TRUE; }; /* CMD_FUNCTION : BOOL CommandSwitchOnChopperController(CommandParameter paramNotUsed) *********************************************************************************** AUTHOR : Amazy USE : Switches On the Chopper Controller PARAMS : */ BOOL CommandSwitchOnChopperController(CommandParameter paramNotUsed) { //power on SET_BITS(gpMim->ControlWord, K_BMASK_MIM_CHOPPER_POWER_CONTROL, K_BMASK_MIM_CHOPPER_POWER_ON); KS_TaskSleep(50); //wait that the interrupt routine actually changes the relay status //select the nominal coils SET_BITS(gpMim->ControlWord, K_BMASK_MIM_CHOPPER_COIL_MODE_MASK, K_BMASK_MIM_CHOPPER_COIL_NOMINAL); KS_TaskSleep(100); //wait that the interrupt routine actually changes the FPGA control register SET_BITS(gpMim->ControlWord, K_BMASK_MIM_CHOPPER_COIL_MODE_MASK, 0); //update status SET_BIT(gpChopperPidController->TaskStatus, K_BMASK_PID_CTRL_STATUS_POWER_ON); SequencerSendAck(DMC_SWON_CHOP_CONT); return FALSE; }; /* FUNCTION : void SwitchOffChopperController() ******************************************** AUTHOR : Amazy Switches off chopper controller */ void SwitchOffChopperController() { DisableChopperController(); KS_TaskSleep(1); SET_BITS(gpMim->ControlWord, K_BMASK_MIM_CHOPPER_POWER_CONTROL, K_BMASK_MIM_CHOPPER_POWER_OFF); //select the degraded coils such that the other DMC can take control of chopper SET_BITS(gpMim->ControlWord, K_BMASK_MIM_CHOPPER_COIL_MODE_MASK, K_BMASK_MIM_CHOPPER_COIL_DEGRADED); KS_TaskSleep(100); //wait that the interrupt routine actually changes the FPGA control register SET_BITS(gpMim->ControlWord, K_BMASK_MIM_CHOPPER_COIL_MODE_MASK, 0); //update status CLEAR_BIT(gpChopperPidController->TaskStatus, K_BMASK_PID_CTRL_STATUS_POWER_ON); } /* CMD_FUNCTION : BOOL CommandSwitchOffChopperController(CommandParameter paramNotUsed) ************************************************************************************ AUTHOR : Amazy USE : Switches Off the Chopper Controller */ BOOL CommandSwitchOffChopperController(CommandParameter paramNotUsed) { SwitchOffChopperController(); SequencerSendAck(DMC_SWOF_CHOP_CONT); return FALSE; }; /* CMD_FUNCTION : BOOL CommandSetCoilDriveMode(CommandParameter param) ******************************************************************* AUTHOR : Amazy USE : Set Chopper coil drive mode */ BOOL CommandSetCoilDriveMode(CommandParameter param) { if (TEST_ONE_BIT(gpChopperPidController->PidStatus, K_BMASK_PID_CTRL_STATUS_PID_ENABLED)) { SequencerSendNack(0, COULD_NOT_EXECUTE_COMMAND); SetSequencerError(ERR_SEQUENCER_COULD_NOT_EXECUTE_COMMAND); return FALSE; } //change the status of the relays selecting the drive SET_BITS(gpMim->ControlWord, K_BMASK_MIM_CHOPPER_COIL_MODE_MASK, param.Uint); KS_TaskSleep(100); SET_BITS(gpMim->ControlWord, K_BMASK_MIM_CHOPPER_COIL_MODE_MASK, 0); // set the open-loop mode if bit 8 is set to one, closed loop if not if (TEST_ONE_BIT(param.Uint, 0x100)) { SET_BIT(gpChopperPidController->PidStatus, K_BMASK_CHOP_CTRL_STATUS_OPEN_LOOP); SET_BIT(gpChopperPidController->TaskStatus, K_BMASK_CHOP_CTRL_STATUS_OPEN_LOOP); } else { CLEAR_BIT(gpChopperPidController->PidStatus, K_BMASK_CHOP_CTRL_STATUS_OPEN_LOOP); CLEAR_BIT(gpChopperPidController->TaskStatus, K_BMASK_CHOP_CTRL_STATUS_OPEN_LOOP); } SequencerSendAck(DMC_SET_CHOP_COIL_DRIVE); return FALSE; }; /* CMD_FUNCTION : BOOL CommandEnableChopperController(CommandParameter paramNotUsed) ********************************************************************************* AUTHOR : Amazy USE : Enable the Chopper Controller */ BOOL CommandEnableChopperController(CommandParameter paramNotUsed) { gpMim->ChopperPreviousPosition = (float)(gpChopperPidController->CurrentPosition); gpMim->ChopperCurrentPosition = (float)(gpChopperPidController->CurrentPosition); gpMim->ChopperPreviousError = 0.0f; gpMim->ChopperCurrentError = 0.0f; gpMim->ChopperAveragedSpeed = 0.0f; gpMim->ChopperAveragedOutput = 0.0f; gpMim->ChopperAveragedDError = 0.0f; gpMim->ChopperOut = 0.0f; gpMim->ChopperPrev1Out = 0.0f; gpMim->ChopperPrev2Out = 0.0f; gpMim->ChopperPrev3Out = 0.0f; gpMim->ChopperPrev4Out = 0.0f; gpMim->ChoppperFilterOut = 0.0f; gpMim->ChoppperFilterPrev1Out = 0.0f; gpMim->ChoppperFilterPrev2Out = 0.0f; gpMim->ChoppperFilterPrev3Out = 0.0f; gpMim->ChoppperFilterPrev4Out = 0.0f; ComputeFloatChopperControllerParams(gpChopperController); if (EnablePidController(gpChopperPidController)) { SequencerSendAck(DMC_ENABLE_CHOP_CONT); VALIDATE_HK(DMC_CHOP_PID_ERR); VALIDATE_HK(DMC_CHOP_PID_ACC); VALIDATE_HK(DMC_CHOP_OUTPUT); VALIDATE_HK(DMC_CHOP_SETPOIN); VALIDATE_HK(DMC_CHOP_TARGET); } else { SequencerSendNack(0, COULD_NOT_EXECUTE_COMMAND); SetSequencerError(ERR_SEQUENCER_COULD_NOT_EXECUTE_COMMAND); } return FALSE; }; /* CMD_FUNCTION : BOOL CommandDisableChopperController(CommandParameter paramNotUsed) ********************************************************************************** AUTHOR : Amazy USE : Disable the Chopper Controller */ BOOL CommandDisableChopperController(CommandParameter paramNotUsed) { DisableChopperController(); CLEAR_BIT(gpChopperPidController->PidStatus, K_BMASK_CHOP_CTRL_STATUS_OPEN_LOOP); CLEAR_BIT(gpChopperPidController->TaskStatus, K_BMASK_CHOP_CTRL_STATUS_OPEN_LOOP); SequencerSendAck(DMC_DISABLE_CHOP_CONT); return FALSE; }; /* FUNCTION : void DisableChopperController() ****************************************** AUTHOR : Amazy Disables chopper controller */ void DisableChopperController() { // disable pid and commutation SET_BITS(gpChopperPidController->ControlWord, K_BMASK_PID_CTRL_COMMAND_ENABLE_COMMAND | K_BMASK_PID_CTRL_COMMAND_COMMUTATAION_COMMAND, K_BMASK_PID_CTRL_COMMAND_DISABLE_PID | K_BMASK_PID_CTRL_COMMAND_DISABLE_COMMUTATION); #ifdef SIMULATOR CLEAR_BIT(gpChopperPidController->PidStatus, K_BMASK_PID_CTRL_STATUS_PID_ENABLED); #endif INVALIDATE_HK(DMC_CHOP_PID_ERR); INVALIDATE_HK(DMC_CHOP_PID_ACC); INVALIDATE_HK(DMC_CHOP_OUTPUT); INVALIDATE_HK(DMC_CHOP_SETPOIN); INVALIDATE_HK(DMC_CHOP_TARGET); } /* Initialize the DitheringDistribution. Note : these values have been provided by HF (mail from 31/07/01). */ int gaDitheringDistribution[] = { -5505, -26741, 16804, 1946, 28209, -7635, 10087, -28387, 14592, 11216, -7640, 8627, 25212, 1272, 9930, -17185, -15568, 17183, 16604, 26818, -28004, -14896, 26061, -14752, 1068, -9223, -16578, -884, 22686, 21690, -30502, 32181, 11750, 17465, -28799, -7593, -1459, 29345, 28488, 26519, -11860, 13185, -32263, -8825, -15326, 31893, -14609, 8490, -21856, 15472, -4167, 31624, 15481, -17485, -26822, 296, -395, -29486, -2330, 1765, -5762, -12693, 26891, 17708, 1950, 2445, 5831, 12240, 27120, -21081, 25465, 21484, 46, 4696, -30602, -15118, 14773, 870, 25607, 16272, -18825, -29657, 22416, 3577, 19818, -29264, -32571, 12351, -31728, -17045, 5971, 8182, -5408, -2103, -5545, 11885, 2258, -31007, -11958, -24552, -11257, 22648, -27935, 22674, 3680, -23099, 5905, 9965, 32732, -9764, 24133, 31674, -753, 22381, -5617, -7358, 28900, 17139, -22694, -23474, 19862, 13751, -20913, 29843, 9913, -16245, -22297, -1944 };