/* * Spindle.c * * Created on: Feb 10, 2025 * Author: Thorsten */ /*! \file */ #include "Spindle.h" #include "FreeRTOS.h" #include "task.h" #include "semphr.h" #include "queue.h" #include "timers.h" #include #include #include #include #include #include // singleton instance pointer // -------------------------------------------------------------------------------------------------------------------- static SpindleHandle_t SpindleInstancePointer = NULL; // -------------------------------------------------------------------------------------------------------------------- typedef enum // -------------------------------------------------------------------------------------------------------------------- { cctNONE = 0x00, cctSTART = 0x01, cctSTOP = 0x02, cctSTATUS = 0x04, } CtrlCommandType_t; // -------------------------------------------------------------------------------------------------------------------- typedef struct StepCommandResponse // -------------------------------------------------------------------------------------------------------------------- { int code; int requestID; union { struct { float speed; int running; } asStatus; } args; } StepCommandResponse_t; // -------------------------------------------------------------------------------------------------------------------- typedef struct CtrlCommand // -------------------------------------------------------------------------------------------------------------------- { struct { int requestID; CtrlCommandType_t type; } head; struct { union { struct { float speed; } asStart; } args; SemaphoreHandle_t syncEvent; } request; StepCommandResponse_t* response; } CtrlCommand_t; // -------------------------------------------------------------------------------------------------------------------- typedef struct stepSyncEventElement // -------------------------------------------------------------------------------------------------------------------- { struct { int allocated; SemaphoreHandle_t event; #if defined(configQUEUE_REGISTRY_SIZE) && (configQUEUE_REGISTRY_SIZE > 0) char lockName[32+1]; #endif } content; LIST_ENTRY(stepSyncEventElement) navigate; } stepSyncEventElement_t; // -------------------------------------------------------------------------------------------------------------------- struct SpindleHandle // -------------------------------------------------------------------------------------------------------------------- { int nextRequestID; ConsoleHandle_t consoleH; TaskHandle_t tHandle; QueueHandle_t cmdQueue; int cancel; SpindlePhysicalParams_t physical; float currentSpeed; struct { SemaphoreHandle_t lockGuard; LIST_HEAD(pool_list, stepSyncEventElement) pool; } syncEventPool; }; // -------------------------------------------------------------------------------------------------------------------- static void SpindleFunction( void * arg ) // -------------------------------------------------------------------------------------------------------------------- { CtrlCommand_t cmd; StepCommandResponse_t asyncResponse; SpindleHandle_t h = (SpindleHandle_t)arg; unsigned int running = 0; unsigned int startupBoost = 0; int dir = h->currentSpeed < 0.0f; h->physical.enaPWM(h, h->physical.context, 0); h->physical.setDutyCycle(h, h->physical.context, 0, 0.0f ); h->currentSpeed = 0; // now here comes the command processor part while( !h->cancel ) { // wait for next command if ( xQueueReceive( h->cmdQueue, &cmd, 100) == pdPASS ) { if ( cmd.response == NULL || cmd.request.syncEvent == NULL ) { cmd.response = &asyncResponse; } memset(cmd.response, 0, sizeof(StepCommandResponse_t)); cmd.response->code = -1; cmd.response->requestID = cmd.head.requestID; switch ( cmd.head.type ) { case cctNONE: cmd.response->code = 0; break; case cctSTART: cmd.response->code = 0; if ( cmd.request.args.asStart.speed < h->physical.minRPM ) cmd.request.args.asStart.speed = h->physical.minRPM; if ( cmd.request.args.asStart.speed > h->physical.maxRPM ) cmd.request.args.asStart.speed = h->physical.maxRPM; if ( cmd.request.args.asStart.speed > 0.0f && cmd.request.args.asStart.speed < h->physical.absMinRPM ) cmd.request.args.asStart.speed = h->physical.absMinRPM; if ( cmd.request.args.asStart.speed < 0.0f && cmd.request.args.asStart.speed > -h->physical.absMinRPM ) cmd.request.args.asStart.speed = -h->physical.absMinRPM; int directionChange = 0; if ((h->currentSpeed < 0.0f && cmd.request.args.asStart.speed > 0.0f) || (h->currentSpeed > 0.0f && cmd.request.args.asStart.speed < 0.0f)) directionChange = 1; h->currentSpeed = cmd.request.args.asStart.speed; dir = h->currentSpeed < 0.0f; float pwm = fabsf(h->currentSpeed) / h->physical.maxRPM; h->physical.setDutyCycle(h, h->physical.context, dir, pwm ); if ( running == 1 && directionChange == 0 ) { h->physical.setDutyCycle(h, h->physical.context, dir, pwm); startupBoost = 0; } else if ( ( running == 0 || directionChange == 1 ) && fabsf(cmd.request.args.asStart.speed) <= (0.25f * h->physical.maxRPM) ) { h->physical.setDutyCycle(h, h->physical.context, dir, 0.5f ); startupBoost = 1; } if ( startupBoost == 0 ) { h->physical.setDutyCycle(h, h->physical.context, dir, pwm); } h->physical.enaPWM(h, h->physical.context, 1); running = 1; break; case cctSTOP: cmd.response->code = 0; h->currentSpeed = 0; startupBoost = 0; running = 0; dir = h->currentSpeed < 0.0f; h->physical.setDutyCycle(h, h->physical.context, dir, 0.0f ); h->physical.enaPWM(h, h->physical.context, 0); break; case cctSTATUS: cmd.response->code = 0; cmd.response->args.asStatus.running = running; cmd.response->args.asStatus.speed = h->currentSpeed; break; default: break; } // after processing the command we have to release the caller to keep // synchronous calling mechanism. In case there is no sync object, it was## // called asynchronously if ( cmd.request.syncEvent != NULL ) { xSemaphoreGive(cmd.request.syncEvent); } } else { // here we have to do some additional steps to regulate correct rpm in case // the low speed boost has been performed if ( startupBoost == 1 && running == 1 ) { startupBoost = 0; h->physical.setDutyCycle(h, h->physical.context, dir, ( fabsf(h->currentSpeed) / h->physical.maxRPM) ); } } } } // -------------------------------------------------------------------------------------------------------------------- static SemaphoreHandle_t GetCommandEvent( SpindleHandle_t h ) // -------------------------------------------------------------------------------------------------------------------- { xSemaphoreTakeRecursive( h->syncEventPool.lockGuard, -1 ); stepSyncEventElement_t* el = LIST_FIRST(&h->syncEventPool.pool); while ( el != NULL ) { if ( el->content.allocated == 0 ) { el->content.allocated = 1; // make sure we the event is in held state xSemaphoreTake( el->content.event, 0 ); xSemaphoreGiveRecursive( h->syncEventPool.lockGuard ); return el->content.event; } el = LIST_NEXT(el, navigate); } xSemaphoreGiveRecursive( h->syncEventPool.lockGuard ); return 0; } // -------------------------------------------------------------------------------------------------------------------- static void ReleaseCommandEvent( SpindleHandle_t h, SemaphoreHandle_t s ) // -------------------------------------------------------------------------------------------------------------------- { xSemaphoreTakeRecursive( h->syncEventPool.lockGuard, -1 ); stepSyncEventElement_t* el = LIST_FIRST(&h->syncEventPool.pool); while ( el != NULL ) { if ( el->content.allocated == 1 && el->content.event == s) { el->content.allocated = 0; xSemaphoreGiveRecursive( h->syncEventPool.lockGuard ); return; } el = LIST_NEXT(el, navigate); } xSemaphoreGiveRecursive( h->syncEventPool.lockGuard ); } // -------------------------------------------------------------------------------------------------------------------- static int SpindleConsoleFunction( int argc, char** argv, void* ctx ) // -------------------------------------------------------------------------------------------------------------------- { //possible commands are //(spindle) start 100 //(spindle) stop //(spindle) status SpindleHandle_t h = (SpindleHandle_t)ctx; StepCommandResponse_t response = { 0 }; CtrlCommand_t cmd; cmd.response = &response; cmd.head.requestID = h->nextRequestID; h->nextRequestID += 1; // first decode the subcommand and all arguments if ( argc == 0 ) { printf("invalid number of arguments\r\nFAIL"); return -1; } if ( strcmp(argv[0], "stop") == 0 ) { // no further arguments cmd.head.type = cctSTOP; } else if ( strcmp(argv[0], "start") == 0 ) { // rpm value directly after start cmd.head.type = cctSTART; cmd.request.args.asStart.speed = 600.0f; if ( argc < 2 ) { printf("missing RPM value for start command\r\nFAIL"); return -1; } cmd.request.args.asStart.speed = (float)atof(argv[1]); } else if ( strcmp(argv[0], "status") == 0 ) { // no further arguments, everything in result cmd.head.type = cctSTATUS; } else { printf("passed invalid sub command\r\nFAIL"); return -1; } // now pass the request to the controller cmd.request.syncEvent = GetCommandEvent(h); if ( pdPASS != xQueueSend( h->cmdQueue, &cmd, -1 ) ) { ReleaseCommandEvent(h, cmd.request.syncEvent ); return -1; } xSemaphoreTake( cmd.request.syncEvent, -1 ); ReleaseCommandEvent(h, cmd.request.syncEvent ); // now decode the result in case there is one if ( response.code == 0 ) { if ( cmd.head.type == cctSTATUS ) { printf("%d\r\n", !!cmd.response->args.asStatus.running); printf("%d\r\n", (int)cmd.response->args.asStatus.speed); } printf("OK"); } else { printf("error returned\r\nFAIL"); } // now back to console return response.code; } // -------------------------------------------------------------------------------------------------------------------- static void SpindleRegisterBasicCommands( SpindleHandle_t h, ConsoleHandle_t cH ) // -------------------------------------------------------------------------------------------------------------------- { CONSOLE_RegisterCommand(cH, "spindle", "<> is used to control a spindle motor.\r\nValid subcommands are start, stop, status.\r\nStart needs an additional RPM argument!", SpindleConsoleFunction, h); } // -------------------------------------------------------------------------------------------------------------------- void SPINDLE_GetStatus(SpindleHandle_t h, int *enabled, int *rpm) // -------------------------------------------------------------------------------------------------------------------- { if (h == NULL) return; if (enabled) *enabled = ((int)h->currentSpeed) != 0; if (rpm) *rpm = (int)h->currentSpeed; } // -------------------------------------------------------------------------------------------------------------------- SpindleHandle_t SPINDLE_CreateInstance( unsigned int uxStackDepth, int xPrio, ConsoleHandle_t cH, SpindlePhysicalParams_t* p ) // -------------------------------------------------------------------------------------------------------------------- { #define ON_NULL_GOTO_ERROR(x) do { if ((x) == NULL) goto error; } while(0); // singleton pattern if ( SpindleInstancePointer != NULL ) return SpindleInstancePointer; if ( p == NULL || p->enaPWM == NULL || p->minRPM >= p->maxRPM || p->setDutyCycle == NULL || cH == NULL ) return NULL; struct SpindleHandle* h = calloc(1, sizeof(struct SpindleHandle)); ON_NULL_GOTO_ERROR(h); if ( h == NULL ) return NULL; h->consoleH = cH; h->cancel = 0; h->nextRequestID = 0; h->cmdQueue = xQueueCreate(16, sizeof(CtrlCommand_t)); ON_NULL_GOTO_ERROR(h->cmdQueue); // copy arguments memcpy(&h->physical, p, sizeof(SpindlePhysicalParams_t)); // now we create the sync event pool LIST_INIT(&h->syncEventPool.pool); h->syncEventPool.lockGuard = xSemaphoreCreateRecursiveMutex(); ON_NULL_GOTO_ERROR(h->syncEventPool.lockGuard); for ( int i = 0; i < 8; i++) { stepSyncEventElement_t* el = (stepSyncEventElement_t*)calloc(1, sizeof(stepSyncEventElement_t)); ON_NULL_GOTO_ERROR(el); el->content.allocated = 0; el->content.event = xSemaphoreCreateBinary(); if (el->content.event == NULL) { free(el); el = NULL; goto error; } else { #if defined(configQUEUE_REGISTRY_SIZE) && (configQUEUE_REGISTRY_SIZE > 0) snprintf(el->content.lockName, sizeof(el->content.lockName)-1, "semSpindlePoolGuard%i", i); vQueueAddToRegistry(el->content.event, el->content.lockName); #endif LIST_INSERT_HEAD(&h->syncEventPool.pool, el, navigate); } } // setup the console commands SpindleRegisterBasicCommands(h, cH); SpindleInstancePointer = h; // setup the task which handles all communications and the RPM generation xTaskCreate(SpindleFunction, "spindlectrl", uxStackDepth, h, xPrio, &h->tHandle); ON_NULL_GOTO_ERROR(h->tHandle); #if defined(configQUEUE_REGISTRY_SIZE) && (configQUEUE_REGISTRY_SIZE > 0) vQueueAddToRegistry(h->cmdQueue, "spindleCmdQ"); vQueueAddToRegistry(h->syncEventPool.lockGuard, "spindleCmdPoolLockGuard"); #endif return h; error: if (h != NULL) { if (h->cmdQueue != NULL) { vQueueDelete(h->cmdQueue); h->cmdQueue = NULL; } if (h->syncEventPool.lockGuard != NULL) { vSemaphoreDelete(h->syncEventPool.lockGuard); h->syncEventPool.lockGuard = NULL; } // first clean all event elements stepSyncEventElement_t* el = NULL; stepSyncEventElement_t* tel = NULL; for (el = LIST_FIRST(&h->syncEventPool.pool); el && (tel = LIST_NEXT(el, navigate), 1); el = tel) { if (el->content.event != NULL) { vSemaphoreDelete(el->content.event); el->content.event = NULL; } } // now remove all elements one by one from the list and free them while (!LIST_EMPTY(&h->syncEventPool.pool)) { stepSyncEventElement_t* elm = LIST_FIRST(&h->syncEventPool.pool); if (elm != NULL) { LIST_REMOVE(elm, navigate); free(elm); } } free(h); } return NULL; }