/*****************************************************************************/ /* Copyright 1998 MBARI */ /*****************************************************************************/ /* Summary : Power Management Module for Tiburon */ /* Filename : power.c */ /* Author : Andrew Pearce */ /* Project : Tiburon */ /* Version : Version 2.0 */ /* Created : 03/05/98 */ /* Modified : 03/05/98 */ /* Archived : */ /*****************************************************************************/ /* Modification History : */ /* $Header$ * $Log$ * */ /*****************************************************************************/ #include /* VxWorks system declarations */ #include /* VxWorks internet protocol definitions */ #include /* VxWorks network socket functions */ #include /* VxWorks select IO facilities */ #include /* VxWorks floating point math library */ #include /* VxWorks log message facilities */ #include #include #include /* MBARI time and date functions */ #include /* C++ Data Manager Classes */ #include /* C++ Data Manager Group Classes */ #include /* Power Manager data type definitions */ #include /* Power Manager data manager definitions */ #include /* Power Manager power allocation */ #include "power.h" /* Power Manager Application definitions */ #include /* Thruster Data Manager Item Names */ #include "powerMgrClass.h" /* Power Manager Classes */ #define POWER_MGR_SWITCH_PORT 5800 /* Power Manager Switch Request Port */ #define POWER_MGR_THRUST_PORT 5801 /* Power Manager Thruster Request Port */ #define REQ_BUF_LEN 256 // Forward declarations for module local functions MLocal STATUS createThrusterDmItems( DmBooleanObject *thrustersPwrReducedDm, DmBooleanObject *thrustersPwrOkDm, ThrusterPwrMgr *thrusters[NUM_THRUSTERS] ); MLocal MBool powerThrusters(Nat16 powerReq, DmNat16Object *thrustersPwrAllocDm, Nat16 *powerAlloc, Nat16 *pwrAvailable); MLocal Nat16 maxPowerChanged(DmNat16Object *maxPowerAvailDm, DmNat16Object *hotelPwrAllocDm, DmNat16Object *lightsPwrAllocDm, DmNat16Object *toolsPwrAllocDm, DmNat16Object *thrustersPwrAllocDm); // ********************************************************************** MLocal Socket createSocket(Int32 type, Int32 port) { Socket sock; struct sockaddr_in sockAddr; Int32 on = 1; /* For setsockopt */ sock = socket(AF_INET, type, 0); if (sock == ERROR) { logMsg("Can't create socket\n"); return(ERROR); } /* if */ bzero ((char *) &sockAddr, sizeof(sockAddr)); sockAddr.sin_family = AF_INET; sockAddr.sin_addr.s_addr = htonl(INADDR_ANY); sockAddr.sin_port = htons(port); if (setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, (char *) &on, sizeof(on)) == ERROR) logMsg("Set sock opt failed\n"); if ( bind(sock, (sockaddr *) &sockAddr, sizeof(sockAddr)) != OK ) { logMsg("Couldn't bind socket to port\n"); close( sock ); return( ERROR ); } /* if */ return (sock); } /* createSocket() */ /*****************************************************************************/ /* Function : powerMgrTask */ /* Purpose : Manages power allocation requests from the various vehicle */ /* subsystems and devices, and the thruster control system. */ /* Waits on a socket for power manager requests from clients */ /* Inputs : None. */ /* Outputs : Returns ERROR on fatal error. */ /*****************************************************************************/ STATUS powerMgrTask( Void ) { Socket switchPwrReqSock; /* Power switch request socket */ Socket thrustPwrReqSock; /* thruster power request socket */ struct sockaddr_in clientAddr; /* Client socket address */ int clientAddrLen; /* Client socket address length */ fd_set fdSet; /* Set of socket fd's for select */ Int32 maxFd; Int32 status; char reqBuf[REQ_BUF_LEN]; /* Power Request Buffer */ switchPwrReqSock = createSocket(SOCK_DGRAM, POWER_MGR_SWITCH_PORT); thrustPwrReqSock = createSocket(SOCK_DGRAM, POWER_MGR_THRUST_PORT); if ((switchPwrReqSock == ERROR) || (thrustPwrReqSock == ERROR)) { logMsg("Power Manager: error creating sockets\n"); return (ERROR); } /* if */ FOREVER /* Wait for connection requests on all */ { /* sockets using select */ errnoSet (OK); /* set error number to OK */ FD_ZERO(&fdSet); /* Zero file descriptor set for select */ FD_SET(switchPwrReqSock, &fdSet); FD_SET(thrustPwrReqSock, &fdSet); maxFd = max(switchPwrReqSock, thrustPwrReqSock) + 1; if (select(maxFd, &fdSet, NULL, NULL, NULL) == ERROR) { /* Select failed so clean up and exit */ logMsg ("Power Manager: select failed\n"); close( switchPwrReqSock ); close( thrustPwrReqSock ); return(ERROR); } /* if */ /* Connect request was received on one */ /* of the sockets that we are monitoring */ /* Check for thruster power request */ if (FD_ISSET(thrustPwrReqSock, &fdSet)) { /* Thruster Request received so read it */ } /* if */ /* Check for switch power request */ if (FD_ISSET(switchPwrReqSock, &fdSet)) { /* Switch Request received so read it */ clientAddrLen = sizeof(clientAddr); if (recvfrom(switchPwrReqSock, reqBuf, REQ_BUF_LEN, 0, (sockaddr *) &clientAddr, &clientAddrLen) != ERROR) { printf("req: %s\n", reqBuf); } /* if */ status = OK; if (sendto(switchPwrReqSock, (char *) &status, sizeof(status), 0, (sockaddr *) &clientAddr, sizeof(clientAddr)) == ERROR) printf ("sendto failed\n"); } /* if */ } /* FOREVER */ DM_Time pwrMgrInitTime; gettimeofday(&pwrMgrInitTime, (struct timezone *) NULL); DmGroup *monitoredItems = new DmGroup(); Nat16 zeroPower = 0; /* initial power allocated */ Nat16 thrustersPowerAlloc = 0; /* Power allocated to thrusters */ Nat16 powerAvailable = MAX_POWER; /* max power available */ DmNat16Object *maxPowerAvailDm = new DmNat16Object(MAX_PWR_AVAIL_DM); maxPowerAvailDm->startProvide(DM_STATIC); maxPowerAvailDm->write(powerAvailable, &pwrMgrInitTime); maxPowerAvailDm->startConsume(DM_STATIC); monitoredItems->addItem(maxPowerAvailDm); DmNat16Object *thrustersPwrAllocDm = new DmNat16Object(THRUSTERS_PWR_ALLOC_DM); thrustersPwrAllocDm->startProvide(DM_STATIC); thrustersPwrAllocDm->write(zeroPower, &pwrMgrInitTime); thrustersPwrAllocDm->startConsume(DM_STATIC); DmNat16Object *hotelPwrAllocDm = new DmNat16Object(HOTEL_PWR_ALLOC_DM); hotelPwrAllocDm->startProvide(DM_STATIC); hotelPwrAllocDm->write(zeroPower, &pwrMgrInitTime); hotelPwrAllocDm->startConsume(DM_STATIC); DmNat16Object *lightsPwrAllocDm = new DmNat16Object(LIGHTS_PWR_ALLOC_DM); lightsPwrAllocDm->startProvide(DM_STATIC); lightsPwrAllocDm->write(zeroPower, &pwrMgrInitTime); lightsPwrAllocDm->startConsume(DM_STATIC); DmNat16Object *toolsPwrAllocDm = new DmNat16Object(TOOLS_PWR_ALLOC_DM); toolsPwrAllocDm->startProvide(DM_STATIC); toolsPwrAllocDm->write(zeroPower, &pwrMgrInitTime); toolsPwrAllocDm->startConsume(DM_STATIC); DmBooleanObject *thrustersPwrReducedDm; DmBooleanObject *thrustersPwrOkDm; ThrusterPwrMgr *thrusters[NUM_THRUSTERS]; // create Thruster Power Manager Objects createThrusterDmItems( thrustersPwrReducedDm, thrustersPwrOkDm, thrusters ); FOREVER { // semTake(dmWakeupSem, WAIT_FOREVER); monitoredItems->getChanges(); if (monitoredItems->itemChanged(maxPowerAvailDm)) powerAvailable = maxPowerChanged( maxPowerAvailDm, hotelPwrAllocDm, lightsPwrAllocDm, toolsPwrAllocDm, thrustersPwrAllocDm ); } /* if */ // service control system thrusters power request Nat16 thrustersPwrRequest; powerThrusters(thrustersPwrRequest, thrustersPwrAllocDm, &thrustersPowerAlloc, &powerAvailable); return(OK); /* We never get here but just to be tidy */ } /* powerMgrTask() */ /*****************************************************************************/ /* Function : testPowerMgr */ /* Purpose : tests power manager power requests */ /* Inputs : None. */ /* Outputs : None */ /*****************************************************************************/ STATUS testPowerMgr( Void ) { Socket switchPwrReqSock; /* Power switch request socket */ struct sockaddr_in pwrMgrAddr; /* Power Manager socket address */ int pwrMgrAddrLen; /* Power Manager Address length */ char reqBuf[REQ_BUF_LEN]; /* Power Request Buffer */ Int32 status; switchPwrReqSock = createSocket(SOCK_DGRAM, 0); if (switchPwrReqSock == ERROR) { logMsg("Power Mgr Test: can't create UDP socket\n"); return(ERROR); } /* if */ /* Set up power mgr socket */ bzero ((char *) &pwrMgrAddr, sizeof(pwrMgrAddr)); pwrMgrAddr.sin_family = AF_INET; pwrMgrAddr.sin_addr.s_addr = htonl(INADDR_ANY); pwrMgrAddr.sin_port = htons(POWER_MGR_SWITCH_PORT); sprintf(reqBuf, "This is a power manager test\n"); if (sendto(switchPwrReqSock, reqBuf, REQ_BUF_LEN, 0, (sockaddr *) &pwrMgrAddr, sizeof(pwrMgrAddr)) != ERROR) { pwrMgrAddrLen = sizeof(pwrMgrAddr); recvfrom(switchPwrReqSock, (char *) &status, sizeof(status), 0, (sockaddr *) &pwrMgrAddr, &pwrMgrAddrLen); printf ("%d\n", status); } /* if */ return(OK); } /* testPowerMgr() */ #if 0 switchDivisions divisions; /* switch load divisions power */ /* initialize switch load divisions cumulative power */ divisions.hotelPwrAlloc = 0; divisions.lightsPwrAlloc = 0; divisions.toolsPwrAlloc = 0; /* service static loads */ for (item = 0; item < powerDmi.numStatic; item++) if (staticGroupBits & powerGroups.staticBit[item]) powerStatic(powerDmi.staticPwrReq[item], powerDmi.staticPwrOk[item], &staticPwrAlloc[item], &powerAvailable, &divisions, staticPwrReq[item]); /* service switched loads */ powerSwitch(powerDmi.switchPwrReq[item], powerDmi.switchPwrOk[item], &switchPwrAlloc[item], &powerAvailable, &divisions, &thrusters); #endif /*****************************************************************************/ /* Function : powerThrusters */ /* Purpose : Allocates available power for thruster control. */ /* Inputs : Thrusters' power request, power allocated and power OK data */ /* manager items, thrusters' power allocated and power available. */ /* Outputs : None. */ /*****************************************************************************/ MLocal MBool powerThrusters(Nat16 powerReq, DmNat16Object *thrustersPwrAllocDm, Nat16 *powerAlloc, Nat16 *pwrAvailable) { /* power requested by control system */ MBool pwrOk; /* power request OK */ Nat16 dpower; /* delta power */ if (powerReq <= *powerAlloc) /* requested less power than is allocated */ { dpower = *powerAlloc - powerReq; *pwrAvailable += dpower; *powerAlloc = powerReq; pwrOk = TRUE; } else /* requested more power than already allocated*/ { if ((dpower = (powerReq - *powerAlloc)) <= *pwrAvailable) { /* requested is */ *pwrAvailable -= dpower; /* available */ *powerAlloc = powerReq; pwrOk = TRUE; } else /* partial power requested is available*/ { *powerAlloc += *pwrAvailable; *pwrAvailable = 0; pwrOk = FALSE; } } /* else*/ return (pwrOk); } /* powerThrusters */ /*****************************************************************************/ /* Function : maxPowerChanged */ /* Purpose : Changes the max power available for allocation. */ /* Inputs : Static load power request and power OK data manager items, */ /* power allocated, power available, switch load divisions */ /* cumulative power pointer, load name. */ /* Outputs : Max power availble. */ /*****************************************************************************/ MLocal Nat16 maxPowerChanged(DmNat16Object *maxPowerAvailDm, DmNat16Object *hotelPwrAllocDm, DmNat16Object *lightsPwrAllocDm, DmNat16Object *toolsPwrAllocDm, DmNat16Object *thrustersPwrAllocDm) { Nat16 maxPwrAvail; /* max power available */ Nat16 thrustersPwrAlloc; /* thrusters power allocated */ Nat16 hotelPwrAlloc; /* hotel power allocated */ Nat16 lightsPwrAlloc; /* lights power allocated */ Nat16 toolsPwrAlloc; /* tools power allocated */ Int16 powerAvailable; /* power available for allocation */ DM_Time updateTime; /* dm item sample time */ /* get max power available */ maxPowerAvailDm->read( &maxPwrAvail, (DM_Time *) NULL ); /* get power already allocated */ thrustersPwrAllocDm->read( &thrustersPwrAlloc, (DM_Time *) NULL); hotelPwrAllocDm->read( &hotelPwrAlloc, (DM_Time *) NULL); lightsPwrAllocDm->read( &lightsPwrAlloc, (DM_Time *) NULL); toolsPwrAllocDm->read( &toolsPwrAlloc, (DM_Time *) NULL); /* determine the power available */ powerAvailable = maxPwrAvail - thrustersPwrAlloc - hotelPwrAlloc - lightsPwrAlloc - toolsPwrAlloc; if (powerAvailable < 0) { logMsg("Power Manager: max power setting too low, reset to default value\n"); maxPwrAvail = MAX_POWER; gettimeofday( &updateTime, (struct timezone *) NULL); maxPowerAvailDm->write( maxPwrAvail, &updateTime ); powerAvailable = maxPwrAvail - thrustersPwrAlloc - hotelPwrAlloc - lightsPwrAlloc - toolsPwrAlloc; } /* if */ return(powerAvailable); } /* maxPower */ /*****************************************************************************/ /* Function : createThrusterDmItems */ /* Purpose : Creates Thruster Motor data manager items, starts consumers and*/ /* providers and sets up groups. */ /* Inputs : Power data manager items, power request and confirm semaphores,*/ /* power groups and static power request names. */ /* Outputs : Returns data manager error code. */ /*****************************************************************************/ MLocal STATUS createThrusterDmItems( DmBooleanObject *thrustersPwrReducedDm, DmBooleanObject *thrustersPwrOkDm, ThrusterPwrMgr *thrusters[NUM_THRUSTERS] ) { MBool False = FALSE; DM_Time pwrMgrInitTime; gettimeofday(&pwrMgrInitTime, (struct timezone *) NULL); // Create Thruster Reduced Power Data Manager Item thrustersPwrReducedDm = new DmBooleanObject(THRUSTERS_PWR_RED_DM); thrustersPwrReducedDm->startProvide(DM_STATIC); thrustersPwrReducedDm->write(False, &pwrMgrInitTime); // Create Thruster Power OK Data Manager Item thrustersPwrOkDm = new DmBooleanObject(THRUSTERS_PWR_OK_DM); thrustersPwrOkDm->startProvide(DM_STATIC); thrustersPwrOkDm->write(False, &pwrMgrInitTime); // Create Individual Thruster Power Data Manager Items thrusters[PORT_HORIZ] = new ThrusterPwrMgr(PORT_HORIZ_THRUSTER_DM MOTOR_ENABLE_DM, THRUSTER_PORT_HORIZ_PWR_CONF_DM); thrusters[STBD_HORIZ] = new ThrusterPwrMgr(STBD_HORIZ_THRUSTER_DM MOTOR_ENABLE_DM, THRUSTER_STBD_HORIZ_PWR_CONF_DM); thrusters[PORT_VERT] = new ThrusterPwrMgr(PORT_VERT_THRUSTER_DM MOTOR_ENABLE_DM, THRUSTER_PORT_VERT_PWR_CONF_DM); thrusters[STBD_VERT] = new ThrusterPwrMgr(STBD_VERT_THRUSTER_DM MOTOR_ENABLE_DM, THRUSTER_STBD_VERT_PWR_CONF_DM); thrusters[FWD_LATERAL] = new ThrusterPwrMgr(FWD_LATERAL_THRUSTER_DM MOTOR_ENABLE_DM, THRUSTER_FWD_LAT_PWR_CONF_DM); thrusters[AFT_LATERAL] = new ThrusterPwrMgr(AFT_LATERAL_THRUSTER_DM MOTOR_ENABLE_DM, THRUSTER_AFT_LAT_PWR_CONF_DM); } /* createThrusterDmItems */