first commit

This commit is contained in:
fewikay committed 2026-10-16 13:08:06 +02:00
commit 92cbe97616
1978 files changed
+1688498

No files matched your search

@@ -0,0 +1,479 @@
/*
* 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 <malloc.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/queue.h>
#include <math.h>
// 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", "<<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;
}