/****************************************************************************/ /* Copyright (c) 2000 MBARI */ /* MBARI Proprietary Information. All rights reserved. */ /****************************************************************************/ /* Summary : MBARI Tailcone Application - Main C Source file */ /* Filename : tailcone.c */ /* Author : Andrew Pearce */ /* Project : Dorado AUV Tailcone */ /* Version : 1.0 */ /* Created : 01/19/00 */ /* Modified : 01/19/00 */ /* Archived : */ /****************************************************************************/ /* Modification History: */ /****************************************************************************/ #pragma model(196) #pragma nosignedchar #include "C:/C96/INCLUDE/string.h" /* string function library */ #include "const.h" /* Misc. constants - TRUE/FALSE etc */ #include "types.h" /* MBARI style guide declarations */ #include "microasm.h" /* Assembly language functions */ #include "applic.h" /* Microcontroller applications */ #include "malloc.h" /* Malloc support routines */ #include "applay.h" #include "microdef.h" /* Microcontroller definitions */ #include "microlib.h" /* Microcontroller Library decls. */ #include "timer.h" /* Timer library definitions */ #include "ring.h" /* Ring buffer support routines */ #include "syslib.h" /* Board Support library decls. */ #include "ibc_card.h" /* IBC Board Types and Addresses */ #include "ibc_cmd.h" /* Core IBC application definitions */ #include "ibc.h" /* IBC Function Library Definitions */ #include "filter.h" /* IBC General IIR Filter Routines */ #include "cpu196.h" /* IBC 80C196 CPU Board functions */ #include "pic.h" /* Intel 82C59 Interrupt Controller */ #include "low_pwr.h" /* Low Power Switch Board Defs */ #include "quad_serial.h" /* Quad Serial Board Definitions */ #include "scc2698.h" /* Signetics SCC2698 UART Definition*/ #include "stp100.h" /* Stepper Motor Controller */ #include "sv203.h" /* Servo Motor Controller */ #include "tailcone.h" /* Application specific definitions */ /****************************************************************************/ const Char micro_id[] = "87C196KC Tailcone"; const Char softwareRev[] = "$Revision: 1.1 $"; const Char serialNumber[] = "Tailcone 01"; const Char *microSerialNo; MLocal Boolean microEnabled; /* Enables serial command processing*/ typedef IBC_BoardEntry * IBCCardPtr; /* Array of IBC boards in backplane */ IBCCardPtr IBC_CardTable[IBC_MAX_CARDS]; Int16 IBC_CardCount; /* IBC cards found in backplane */ sv203Struct propMotor; /* Tailcone propeller motor struct */ stpStruct elevator; /* Tailcone elevator control surface*/ stpStruct rudder; /* Tailcone rudder control surface */ /* Global Variables */ Int16 cmdSpeed; Int16 measSpeed; /* Motor Controller Gains */ Int16 KmotorInv; Int16 Kp; Int16 Ki; Int16 Kd; MBool deadmanStop; Nat16 LinkTimeout; /****************************************************************************/ /* Function : initializePropMotor */ /* Purpose : Initializes propeller motor controller */ /* Inputs : None */ /* Outputs : None */ /****************************************************************************/ Int16 initializePropMotor(sv203Struct *propMotor, Nat16 serialChan) { sv203OpenSerialChan(propMotor, (Byte*) QUAD_SERIAL_0_ADDR, serialChan, IBC_CardTable, IBC_CardCount ); sv203BoardSelect(propMotor, 1); sv203ServoSelect(propMotor, 1); } /* initializePropMotor() */ /****************************************************************************/ /* Function : initializeControlSurface */ /* Purpose : Initializes actuator controller */ /* Inputs : None */ /* Outputs : None */ /****************************************************************************/ Int16 initializeControlSurface(stpStruct *ctrlSurface, Nat16 serialChan) { stp100OpenSerialChan(ctrlSurface, (Byte*) QUAD_SERIAL_0_ADDR, serialChan, IBC_CardTable, IBC_CardCount ); stp100InitializeController(ctrlSurface, CTRL_SURFACE_CTRL_PERIOD); stp100SetStepMode(ctrlSurface); stp100FindHome(ctrlSurface); stp100StepDelay(ctrlSurface, 256); stp100VelocityProfile(ctrlSurface, 1024, 5);/*2000 Andy's setting*/ } /* initializeControlSurface() */ /****************************************************************************/ /* Function : setMotorSpeedCmd */ /* Purpose : Set prop motor speed */ /* Inputs : Command buffer */ /* Outputs : None */ /* Note : sets motor speed. Speed control is done in motorContro(). */ /****************************************************************************/ void setMotorSpeedCmd(Int16 *commandBuf) { Int16 status; status = 0; /* ** commandBuf[1] contains a signed int; 1 rpm/count. If PDEBUG is set ** then commandBuf[1] is Pontech counts. */ cmdSpeed = commandBuf[1]; sendMVCReply( SET_MOTOR_SPEED, 2, (Word)status, (Word) cmdSpeed); } /* setMotorSpeedCmd() */ /****************************************************************************/ /* Function : setElevatorPositionCmd */ /* Purpose : Set elevator position */ /* Inputs : Command buffer */ /* Outputs : None */ /****************************************************************************/ void setElevatorPositionCmd(Int16 *commandBuf) { MLocal Int16 lastelevatorpos; Int16 status; status = 0; elevator.desiredPosition = commandBuf[1];; if(elevator.desiredPosition != lastelevatorpos) { /* a little redundant but what the hey, as long as the desiredpos is not compared to the commanded pos anywhere in MoveAbsolute we should be ok */ lastelevatorpos = elevator.desiredPosition; stp100SmoothHalt(&elevator); stp100MoveAbsolute(&elevator, elevator.desiredPosition ); } sendMVCReply( SET_ELEVATOR_POS, 2,(Word) status, elevator.desiredPosition); } /* setElevatorPositionCmd() */ /****************************************************************************/ /* Function : setRudderPositionCmd */ /* Purpose : Set rudder position */ /* Inputs : number of steps to move +- */ /* Outputs : None */ /****************************************************************************/ void setRudderPositionCmd(Int16* commandBuf) { MLocal Int16 lastrudderpos; Int16 status; status = 0; rudder.desiredPosition = commandBuf[1]; if(rudder.desiredPosition != lastrudderpos) { /* a little redundant but what the hey, as long as the desiredpos is not compared to the commanded pos anywhere in MoveAbsolute we should be ok */ lastrudderpos = rudder.desiredPosition; /* does this finish before MoveAbsolute gets it going again? */ stp100SmoothHalt(&rudder); stp100MoveAbsolute(&rudder, rudder.desiredPosition); } sendMVCReply( SET_RUDDER_POS, 2, (Word)status, rudder.desiredPosition); } /* setRudderPositionCmd() */ /****************************************************************************/ /* Function : setControlValuesCmd */ /* Purpose : Set elevator and rudder position, motor torque */ /* Inputs : Command buffer */ /* Outputs : None */ /* Note : Sets motor torque, not speed, needs to be upgraded */ /****************************************************************************/ void setControlValuesCmd(Int16 *commandBuf) { Byte torque; MLocal Int16 lastelevatorpos; MLocal Int16 lastrudderpos; Int16 elevatorpos; Int16 rudderpos; Int16 status; status = 0; /* defined in protocol for 0x34 command does not seem to define which */ /* actuator angle is specified first. So we define it here to be elevator */ /* first. This wil be true for the getControl status request as well. */ cmdSpeed = commandBuf[1]; elevatorpos = commandBuf[2]; rudderpos = commandBuf[3]; if(elevatorpos != lastelevatorpos) { elevator.desiredPosition = elevatorpos; stp100SmoothHalt(&elevator); stp100MoveAbsolute(&elevator, elevator.desiredPosition); lastelevatorpos = elevatorpos; } if(rudderpos != lastrudderpos) { rudder.desiredPosition = rudderpos; stp100SmoothHalt(&rudder); stp100MoveAbsolute(&rudder, rudder.desiredPosition); lastrudderpos = rudderpos; } /** I guess the reply should be an echo of the command **/ sendMVCReply( SET_CONTROL_VALUES, 4,(Word)status, (Word)cmdSpeed, (Word) elevatorpos, (Word)rudderpos); } /* setControlValuesCmd() */ /****************************************************************************/ /* Function : initializeTailconeCmd */ /* Purpose : Initialize elevator, rudder, and prop motor */ /* Inputs : Command buffer */ /* Outputs : None */ /****************************************************************************/ void initializeTailconeCmd(Int16 *commandBuf) { /* Set Motor Contorller Gains */ KmotorInv = commandBuf[1]; Kp = commandBuf[2]; Ki = commandBuf[3]; Kd = commandBuf[4]; /* Initialize Controllers */ initializePropMotor(&propMotor, PROP_MOTOR_SER_CHAN); initializeControlSurface(&rudder, RUDDER_SER_CHAN); initializeControlSurface(&elevator, ELEVATOR_SER_CHAN); sendMVCReply( INITIALIZE_TAILCONE, 4, (Word)KmotorInv, (Word)Kp, (Word) Ki,(Word) Kd); } /* initializeTailconeCmd() */ /****************************************************************************/ /* Function : setDeadmanCmd */ /* Purpose : Sets deadman timeout. */ /* Inputs : Command buffer */ /* Outputs : None */ /****************************************************************************/ void setDeadmanCmd(Int16 *commandBuf) { Nat16 timeout; timeout = commandBuf[1]; LinkTimeout = timeout * 10; /* 10 Ticks per Second */ /* setLinkRxTimeout(LinkTimeout); */ sendMVCReply( SET_DEADMAN_TIMEOUT, 2, OK, LinkTimeout); } /* setDeadmanCmd() */ /****************************************************************************/ /* Function : saveConfigCmd */ /* Purpose : Save configuration to non-volatile RAM. */ /* Inputs : Command buffer */ /* Outputs : None */ /****************************************************************************/ void saveConfigCmd(Int16 *commandBuf) { sendMVCReply( SAVE_CONFIG_TO_NVRAM, 4, 1, 32767, 0, 55); } /* saveConfigCmd() */ /****************************************************************************/ /* Function : getMotorRPMCmd */ /* Purpose : Get prop motor rpm via AD converter on SV203. */ /* Inputs : None */ /* Outputs : None */ /****************************************************************************/ void getMotorRPMCmd() { Int16 status; status = 0; sendMVCReply( GET_MOTOR_RPM, 2, (Word)status, (Word)measSpeed); } /* getMotorRPMCmd() */ /****************************************************************************/ /* Function : getActuatorPosCmd */ /* Purpose : Get actuator position via AD converter on STP100. */ /* Inputs : None */ /* Outputs : None */ /****************************************************************************/ void getActuatorPosCmd() { Int16 elvpos = stp100PosInSteps(&elevator); Int16 rudpos = stp100PosInSteps(&rudder); /** sending raw position vals back **/ sendMVCReply( GET_ACTUATOR_POS, 3, OK, (Word)elvpos, (Word)rudpos); } /* getActuatorPosCmd() */ /****************************************************************************/ /* Function : getControlStatusCmd */ /* Purpose : Get status of prop motor and actuators. */ /* Inputs : None */ /* Outputs : None */ /****************************************************************************/ void getControlStatusCmd() { Int16 result; Int16 elvpos = stp100PosInSteps(&elevator); Int16 rudpos = stp100PosInSteps(&rudder); Int16 speed = measSpeed; sendMVCReply( GET_CONTROL_STATUS, 4, result, (Word)speed, (Word)elvpos, (Word)rudpos); } /* getControlStatusCmd() */ /****************************************************************************/ /* Function : getHealthStatusCmd */ /* Purpose : Get health status of tailcone. */ /* Inputs : Command buffer */ /* Outputs : None */ /****************************************************************************/ void getHealthStatusCmd(Int16 *commandBuf) { sendMVCReply( GET_HEALTH_STATUS, 4, 1, 32767, 0, 59); } /* getHealthStatusCmd() */ /****************************************************************************/ /* Function : getEngTelemetryCmd */ /* Purpose : Get engineering telemetry from tailcone. */ /* Inputs : Command buffer */ /* Outputs : None */ /****************************************************************************/ void getEngTelemetryCmd(Int16 *commandBuf) { sendMVCReply( GET_ENG_TELEMETRY, 4, 1, 32767, 0, 60); } /* getEngTelemetryCmd() */ /****************************************************************************/ /* Function : getTemperatureCmd */ /* Purpose : Get temperature from tailcone. */ /* Inputs : Command buffer */ /* Outputs : None */ /****************************************************************************/ void getTemperatureCmd(Int16 *commandBuf) { sendMVCReply( GET_TEMPERATURE, 4, 1, 32767, (Word)10,(Word) 20); } /* getTemperatureCmd() */ /****************************************************************************/ /* Function : roundCurrent */ /* Purpose : Rounds a current reading to nearest 100 mAmps */ /* Inputs : Current value in mAmps */ /* Outputs : Rounded current value in mAmps */ /****************************************************************************/ MLocal Int16 roundCurrent(Int16 value) { if (value >= 0) return (( (value / 100) + ((value % 100) >= 50)) * 100); else return (( (value / 100) - ((abs(value) % 100) >= 50)) * 100); } /* roundCurrent() */ /****************************************************************************/ /* Function : getCurrentReadingsCmd */ /* Purpose : Get current readings from tailcone. */ /* Inputs : Command buffer */ /* Outputs : None */ /****************************************************************************/ void getCurrentReadingsCmd(Int16 *commandBuf) { Int16 card; Int16 chan0current, chan1current, chan2current,chan3current; LPS_BoardEntry *LPS_Board; /* LPS Board Entry data structure */ if ( (card = ibcBoardTableIndex(IBC_CardTable, IBC_CardCount, (Byte *) LOW_PWR_SWITCH_24V_0_ADDR, LOW_POWER_SWITCH) ) != ERROR) LPS_Board = (LPS_BoardEntry *) IBC_CardTable[card]; /* Send filtered current reading */ chan0current = roundCurrent( LPS_Board->lpsChan[0].filteredCurrent); chan1current = roundCurrent( LPS_Board->lpsChan[1].filteredCurrent); chan2current = roundCurrent( LPS_Board->lpsChan[2].filteredCurrent); chan3current = roundCurrent( LPS_Board->lpsChan[3].filteredCurrent), sendMVCReply( GET_CURRENT_READINGS, 5, OK, (Word) chan0current, (Word) chan1current, (Word) chan2current, (Word) chan3current); } /* getCurrentReadingsCmd() */ /****************************************************************************/ /* Function : getGndFaultCmd */ /* Purpose : Get ground fault current from tailcone. */ /* Inputs : Command buffer */ /* Outputs : None */ /****************************************************************************/ void getGndFaultCmd(Int16 *commandBuf) { sendMVCReply( GET_GND_FAULT_CURRENT, 4, 1, 32767, 0, 63); } /* getGndFaultCmd() */ /****************************************************************************/ /* Function : getConfigurationCmd */ /* Purpose : Get tailcone Configuration. */ /* Inputs : Command buffer */ /* Outputs : None */ /****************************************************************************/ void getConfigurationCmd(Int16 *commandBuf) { sendMVCReply( GET_CONFIGURATION, 4, 1, 32767, 0, 64); } /* getConfigurationCmd() */ /****************************************************************************/ /* Function : hextoi */ /* Purpose : Convert an ascii string representing a 4 digit hex num to */ /* : a Word. it is the responsibility of caller to make sure */ /* : buffer has 4 hex chars in it. garbage in garbage out */ /* Inputs : str to convert */ /* Outputs : Word to yo mama */ /****************************************************************************/ Word hextoi(Byte* buffer) { Nat16 i = 0; Int16 retval = 0; Word temp = 0; /* unsigned 16 bit num */ Byte c = 0; /* 4 hex digits represented in ascii make Word */ for(i=0;i<4;i++) { temp = temp<<4; c = buffer[i]; if((c>='0')&&(c<='9')) c = c - 0x30; else if((c>='A')&&(c<='F')) c = c - 0x41 + 10; else if((c>='a')&&(c<='f')) c = c - 0x61 + 10; temp |= c; } retval = (Int16)temp; return (retval); } /****************************************************************************/ /* Function : convertToInts */ /* Purpose : take a buffer of 20 bytes and return a buffer 5 Ints */ /* : assumes word buffer has been alloced */ /* Inputs : Buffer of 20 bytes , buffer of 5 Words */ /* Outputs : None */ /****************************************************************************/ void convertToInts(Byte* bytebuf , Int16* Intbuf) { Nat16 i = 0; Nat16 j = 0; Byte temp3 = 0x0; Int16 temp = 0; Byte temp2[4]; for(i=0;i<5;i++) { temp = 0; for(j=0;j<4;j++) { temp3 = bytebuf[i*4+j]; temp2[j] = temp3; } temp = hextoi(temp2); Intbuf[i] = temp; /* get rid of temp after testing */ } } /****************************************************************************/ /* Function : processApplicCommands */ /* Purpose : Decode application specific serial port commands */ /* Inputs : Command buffer and command buffer length */ /* Outputs : Return TRUE if command recognized and processed, else FALSE*/ /****************************************************************************/ MLocal Int16 processApplicCommands(Int16 *commandBuf) { Int16 command; /* try to recognize the command and call */ /* the appropriate handler */ command = commandBuf[0]; switch(command) { case SET_MOTOR_SPEED: setMotorSpeedCmd(commandBuf); break; case SET_ELEVATOR_POS: setElevatorPositionCmd(commandBuf); break; case SET_RUDDER_POS: setRudderPositionCmd(commandBuf); break; case SET_CONTROL_VALUES: setControlValuesCmd(commandBuf); break; case INITIALIZE_TAILCONE: initializeTailconeCmd(commandBuf); break; case SET_DEADMAN_TIMEOUT: setDeadmanCmd(commandBuf); break; case SAVE_CONFIG_TO_NVRAM: saveConfigCmd(commandBuf); break; case GET_MOTOR_RPM: getMotorRPMCmd(); break; case GET_ACTUATOR_POS: getActuatorPosCmd(); break; case GET_CONTROL_STATUS: getControlStatusCmd(); break; case GET_HEALTH_STATUS: getHealthStatusCmd(commandBuf); break; case GET_ENG_TELEMETRY: getEngTelemetryCmd(commandBuf); break; case GET_TEMPERATURE: getTemperatureCmd(commandBuf); break; case GET_CURRENT_READINGS: getCurrentReadingsCmd(commandBuf); break; case GET_GND_FAULT_CURRENT: getGndFaultCmd(commandBuf); break; case GET_CONFIGURATION: getConfigurationCmd(commandBuf); break; case ERR_INVLD_PKT: sendMVCReply(ERR_INVLD_PKT,4,1,32767,0,0); break; case ERR_UNKN_CMD: sendMVCReply(ERR_UNKN_CMD,4,1,32767,0,0); break; default: sendMVCReply( DEBUG, 4, 1, 32767, 0, 0); return (FALSE); /* Command not recognized return FALSE */ } /* switch */ return (TRUE); /* Command recognized & processed */ } /* processApplicCommands() */ /****************************************************************************/ /* Function : executeCommand */ /* Purpose : Check commands received from serial port and call handler */ /* Inputs : None */ /* Outputs : None */ /****************************************************************************/ Void executeCommand( Void ) { /* array for the command packet data. */ Byte* commandBuf = (Byte*)malloc(MAX_NUM_OF_WORDS); Int16 CommandWords[5]; /* place for the converted 16bit nums */ Nat16 timeouttics = 10; /* 100 tics = 1 sec timeout */ /* read a command from serial port */ while (readMVCPacket(commandBuf,timeouttics) == OK) { convertToInts(commandBuf,&CommandWords); processApplicCommands(&CommandWords); } /* while */ free(commandBuf); } /* executeCommand() */ /****************************************************************************/ /* Function : initializeAlarms */ /* Purpose : Initializes various alarm status and thresholds */ /* Inputs : None */ /* Outputs : None */ /****************************************************************************/ MLocal Void initializeAlarms( Void ) { } /* initializeAlarms() */ MLocal Void switchOnActuators( Void ) { Int16 card; /* Card index into Card Table */ LPS_BoardEntry *lpsBoard; Word chan = 2; if ( (card = ibcBoardTableIndex(IBC_CardTable, IBC_CardCount, (Byte *) LOW_PWR_SWITCH_24V_0_ADDR, LOW_POWER_SWITCH) ) != ERROR) { lpsBoard = (LPS_BoardEntry *) IBC_CardTable[card]; /* Set gang output flags for both channels */ lpsBoard->lpsChan[chan].gangOutputs = TRUE; lpsBoard->lpsChan[chan + 1].gangOutputs = TRUE; /* Switch Vicor Output On */ LPS_BoardVicorCtrl( lpsBoard, ON ); LPS_BoardSwitch( lpsBoard, RUDDER_LPS_CHAN, ON ); LPS_BoardSwitch( lpsBoard, ELEVATOR_LPS_CHAN, ON ); LPS_BoardSwitch( lpsBoard, PROP_MOTOR_LPS_CHAN, ON ); } /* if */ } /* switchOnActuators() */ /****************************************************************************/ /* Function : motorControl */ /* Purpose : Thruster Motor Control Routine */ /* Inputs : None */ /* Outputs : None */ /****************************************************************************/ Void motorControl(Void) { const Int16 offset = MOTOR_OFFSET; const Int32 scale = 10000; const Int16 intStateMax = 20; /* Speed integrator; Max pwm counts */ const Int16 intStateMin = -20; /* Speed integrator; Max pwm counts */ const Int16 mcDeadband = 2; /* Motor controller board deadband */ const Int16 counts2rpm = 4; /* Converts A2D counts to rpm */ Int16 intState = 0; Int16 errorSpeed, pwmCounts; Int32 pwm; MLocal Int16 lastCmdSpeed; MLocal Int16 lastPwmCounts; MLocal Int16 lastMeasSpeed; MLocal Int16 lastIntState; /* ** Read the tach. The conversion is 4 propeller rpm /count, and is an ** unsigned int. The tach doesn't provide a sign for direction. ** ** The speed computations will be done in propeller coordinates. That ** is, commanded and measured speed will refer to the propeller, not the ** motor, which runs 10x faster due to the gearbox. The constant ** KmotorInv contains a factor of 10 so that the final speed command to the ** motor is 10x the commanded speed. */ measSpeed = sv203ReadAtoD( &propMotor, 1 ); measSpeed = counts2rpm * measSpeed; #if 1 /* ** Open loop control: */ pwm = ( (Int32) KmotorInv * (Int32) cmdSpeed ); #else /* ** Closed loop control. This can be put into the open-loop configuration ** setting Kp, Ki and Kd to zero in doradoTailCone.cfg ** ** For the first cut, don't accept a negative speed setpoint because ** the tach can't indicate direction. */ if( cmdSpeed <= 0 ) cmdSpeed = -1 * cmdSpeed; errorSpeed = cmdSpeed - measSpeed; /* ** This is a backward-Euler integrator, that is; z/(z-1). The sampling ** period is subsumed into Ki. The realization is ** x(k) = x(k-1) + u(k) */ intState = lastIntState + errorSpeed; if( intState >= intStateMax ) intState = intStateMax; if( intState <= intStateMin ) intState = intStateMin; #if 0 pwm = ( (Int32) KmotorInv * (Int32) cmdSpeed ) + ( (Int32) Kp * (Int32) errorSpeed ) + ( (Int32) Kd * (Int32) ( measSpeed - lastMeasSpeed ) ) + ( (Int32) Ki * (Int32) intState ); #endif pwm = ( (Int32)(Ki * intState)); lastMeasSpeed = measSpeed; lastIntState = intState; #endif /* ** Convert to 8 bit pwm, shifted up by the offset. The minus sign below ** causes the motor to spin in the forward direction for pwm > 0. */ pwmCounts = offset - ( (Int16) ( pwm / scale) ); /* ** Account for the motor controller board's deadband: */ if( cmdSpeed >= 1 ) pwmCounts = pwmCounts - mcDeadband; if( cmdSpeed <= -1 ) pwmCounts = pwmCounts + mcDeadband; /* ** PDEBUG: cmdSpeed is in units of Pontech counts. */ #if( PDEBUG ) pwmCounts = cmdSpeed; #endif /* ** Don't allow out-of-range commands. */ if( pwmCounts <= 1 ) pwmCounts = 1; if( pwmCounts >= 255 ) pwmCounts = 255; /* ** Write to the Pontech motor controller board */ if(pwmCounts != lastPwmCounts){ sv203SetPWM(&propMotor, (Byte) pwmCounts); lastPwmCounts = pwmCounts; } #if 0 if(cmdSpeed != lastCmdSpeed) { lastCmdSpeed = cmdSpeed; sendMVCReply( SET_MOTOR_SPEED, 1, pwmCounts); } #endif }/*motorControl*/ /****************************************************************************/ /* Function : motorStop */ /* Purpose : Set Motor Torque Command to zero */ /* Inputs : None */ /* Outputs : None */ /****************************************************************************/ MLocal Void motorStop( Void ) { deadmanStop = TRUE; } /* motorStop() */ /****************************************************************************/ /* Function : motorStopFunction */ /* Purpose : Set Motor Torque Command to zero */ /* Inputs : None */ /* Outputs : None */ /****************************************************************************/ MLocal Void motorStopFunction( Void ) { Int16 speed; Int16 pwmCounts; Int16 signFlip = 1; Int32 pwm; const Int16 mcDeadband = 2; /* Motor controller board deadband */ const Int32 scale = 10000; const Int16 speedIncr = 50; speed = cmdSpeed; if( speed < 0 ) { signFlip = -1; } else { signFlip = 1; } while( (signFlip*speed) > 0) { if (abs(speed) < speedIncr) speed = 0; else /* subtract speedIncr RPM from Speed */ speed = speed - (signFlip*speedIncr); /* Set motor Command to Stop */ pwm = ( (Int32) KmotorInv * (Int32) speed ); pwmCounts = MOTOR_OFFSET - ( (Int16) ( pwm / scale) ); if( speed >= 1 ) pwmCounts = pwmCounts - mcDeadband; if( speed <= -1 ) pwmCounts = pwmCounts + mcDeadband; /** Don't allow out-of-range commands. */ if( pwmCounts <= 1 ) pwmCounts = 1; if( pwmCounts >= 255 ) pwmCounts = 255; /* ** Write to the Pontech motor controller board */ sv203SetPWM(&propMotor, (Byte) pwmCounts); taskDelay(50);/* Delay 1/2 second*/ } cmdSpeed = speed; deadmanStop = FALSE; } /* motorStopFunction() */ /****************************************************************************/ /* Function : mainLoop */ /* Purpose : C main provgram called from assembler startup routine */ /* Inputs : None */ /* Outputs : None */ /****************************************************************************/ Void mainLoop( Void ) { char firmware[20]; Int16 psn; initIoport1(); /* Initialize Input/Output Port 1 */ initIoport2(); /* Initialize Input/Output Port 2 */ initMalloc(); /* Initialize Memory allocator */ initBoardStatus(); /* Initialize board status word */ #if PDEBUG cmdSpeed = MOTOR_OFFSET; /* MOTOR_STOP counts => motor stopped */ #else cmdSpeed = 0; /* Motor stopped is the default. */ #endif KmotorInv = 758; /*10-4 counts/rpm; initial value */ Kp = 0; /*10-4 counts/rpm; Prop. Gain, initl val */ Ki = 1000; /*10-4 counts/rpm; Int. Gain, initl val */ Kd = 0; /*10-4 counts/rpm; Der. Gain, initl val */ microEnabled = FALSE; /* Disabled processing of serial commands */ microSerialNo = serialNumber; deadmanStop = FALSE; initSerialProtocol(motorStop);/* Initialize serial protocol */ /*LinkTimeout = getLinkRxTimeout();*/ /* Initialize installed IBC Cards */ IBC_CardCount = IBC_initializeCards( IBC_CardTable, IBC_MAX_CARDS); initializeAlarms(); /* Initialize application specific alarms */ switchOnActuators(); /* Power up actuators */ /* Initialize propeller motor */ initializePropMotor(&propMotor, PROP_MOTOR_SER_CHAN); /* Initialize rudder control surface */ initializeControlSurface(&rudder, RUDDER_SER_CHAN); /* Initialize elevator control surfac */ initializeControlSurface(&elevator, ELEVATOR_SER_CHAN); FOREVER { /* Repeat forever */ executeCommand(); /* Process any commands from the MVC */ if(deadmanStop) motorStopFunction(); else motorControl(); } /* FOREVER */ /* ** Turn the motor off. */ sv203SetPWM(&propMotor, MOTOR_OFFSET); } /* mainLoop() */