You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
377 lines
12 KiB
377 lines
12 KiB
/*
|
|
#include <splitflap_positions.c>
|
|
#include <splitflap_flaps.h>
|
|
* splitflap.c
|
|
*
|
|
* Created on: 29.09.2022
|
|
* Author: jonas
|
|
*/
|
|
|
|
#include "platform.h"
|
|
#include <stdbool.h>
|
|
#include "splitflap.h"
|
|
#include "splitflap_positions.h"
|
|
#include "McuRTOS.h"
|
|
#include "McuULN2003.h"
|
|
#include <stdbool.h>
|
|
#include "McuWait.h"
|
|
#include "McuLog.h"
|
|
#include "lib/dict.h"
|
|
#include "McuUtility.h"
|
|
#include "McuLib.h"
|
|
#include "MotOffsetTable.h"
|
|
#include "nvmc.h"
|
|
|
|
#define FLAP_QUEUE_LENGTH (5) /* number of elements in queue */
|
|
#define FLAP_QUEUE_SELEM_SIZE (sizeof(Flap_t)) /* size of element */
|
|
|
|
/* function declarations */
|
|
static bool OngoingMoveMutex_Lock(SF_Handle_t instance);
|
|
static void OngoingMoveMutex_Unlock(SF_Handle_t instance);
|
|
static void SF_Task(void *pv);
|
|
|
|
/**********************/
|
|
/* INIT / DEINIT */
|
|
/**********************/
|
|
SF_Handle_t SF_Init(SF_Config_t* config){
|
|
SF_t* splitflap;
|
|
|
|
#if SPLITFLAP_CONFIG_USE_FREERTOS_HEAP
|
|
splitflap = (SF_t*)pvPortMalloc(sizeof(SF_t)); // get memory for device from FreeRtos heap
|
|
#else
|
|
splitflap = (SF_t*)malloc(sizeof(SF_t)); // get memory for device from MCU heap
|
|
#endif
|
|
|
|
splitflap->magSensor = McuGPIO_InitGPIO(&config->magSensorConfig); // initialize magnet sensor GPIO
|
|
splitflap->motor = McuULN2003_InitMotor(&config->motorConfig); // initialize ULN driver for stepper motor
|
|
splitflap->ongoingMoveMutex = xSemaphoreCreateRecursiveMutex(); // create mutex for ongoing move
|
|
splitflap->flapQueue = xQueueCreate(FLAP_QUEUE_LENGTH, // create queue
|
|
FLAP_QUEUE_SELEM_SIZE);
|
|
splitflap->id = config->setupIdentifier; // copy setup identifier
|
|
splitflap->hwId = config->hardwareIdentifier; // copy hardware identifier
|
|
splitflap->state = SF_STATE_NOT_READY; // set to "not initialized"
|
|
#if PL_CONFIG_USE_NVMC
|
|
int16_t hwIdFromNvmc = -1;
|
|
// if there exists a hw id in the NVMC and it is valid
|
|
if(NVMC_GetStepperID(config->setupIdentifier, &hwIdFromNvmc) == ERR_OK && hwIdFromNvmc >= 0){
|
|
splitflap->hwId = hwIdFromNvmc; // use this
|
|
} else{
|
|
McuLog_warn("SF_Init: No or invalid HwId found in NVMC for SplitFlap <%i>, storing default ID.", splitflap->id);
|
|
// save default stepper id from code to NVMC
|
|
if(NVMC_SetStepperID(config->setupIdentifier, config->hardwareIdentifier) != ERR_OK){
|
|
McuLog_error("SF_Init: Failed to save hardware ID in NVMC for SplitFlap ID <%i>", config->setupIdentifier);
|
|
}
|
|
}
|
|
#endif
|
|
// check if queue was created
|
|
if(splitflap->flapQueue == NULL){
|
|
McuLog_error("Flap queue creation failed for Splitflap <%i>", splitflap->id);
|
|
for(;;){}
|
|
}
|
|
// if yes: add queue to registry
|
|
vQueueAddToRegistry(splitflap->flapQueue, "FlapQueue");
|
|
|
|
// check if queue was created
|
|
if(splitflap->ongoingMoveMutex == NULL){
|
|
McuLog_error("Ongoing Move mutex creation failed for Splitflap <%i>", splitflap->id);
|
|
for(;;){}
|
|
}
|
|
// if yes: add ongoing move mutex to registry
|
|
vQueueAddToRegistry(splitflap->ongoingMoveMutex, "OngoingMoveMutex");
|
|
|
|
// create Task for splitflap
|
|
BaseType_t res = xTaskCreate( SF_Task,
|
|
"SF",
|
|
800/sizeof(StackType_t),
|
|
splitflap, // no &, since otherwise pointing on parameter address! we want handle address!
|
|
tskIDLE_PRIORITY,
|
|
NULL);
|
|
if(res != pdPASS) // task creation not successful?
|
|
{
|
|
McuLog_error("Creation of task for Splitflap <%i> failed", splitflap->id);
|
|
for(;;) {} // Endless loop
|
|
}
|
|
|
|
return splitflap;
|
|
}
|
|
|
|
void SF_Deinit(SF_Handle_t instance){
|
|
McuLog_warn("Deinitializing Splitflap <%i>", ((SF_t*)instance)->id);
|
|
vSemaphoreDelete(((SF_t*)instance)->ongoingMoveMutex);
|
|
vQueueDelete(((SF_t*)instance)->flapQueue);
|
|
((SF_t*)instance)->state = SF_STATE_NOT_READY;
|
|
McuULN2003_DeinitMotor(((SF_t*)instance)->motor);
|
|
McuGPIO_DeinitGPIO(((SF_t*)instance)->magSensor);
|
|
#if SPLITFLAP_CONFIG_USE_FREERTOS_HEAP
|
|
vPortFree((SF_t*)instance); // free FreeRTOS heap
|
|
#else
|
|
free((SF_t*)instance); // free MCU heap
|
|
#endif
|
|
}
|
|
|
|
|
|
/**********************/
|
|
/* PUBLIC FUNCTIONS */
|
|
/**********************/
|
|
bool SF_MoveMotorToZeroPosition(SF_Handle_t instance){
|
|
int numStepsMoved = 0;
|
|
int32_t offsetSteps = 0;
|
|
// check if the hwId needs an offset init position
|
|
if(MotOffset_Get(((SF_t*)instance)->hwId, &offsetSteps) == ERR_OK){
|
|
McuLog_info("SF_MoveMotorToZeroPosition found offsetSteps=%i for Splitflap <%i> with hwId=%i", offsetSteps, ((SF_t*)instance)->id, ((SF_t*)instance)->hwId);
|
|
}
|
|
|
|
// check if positive offset!
|
|
if(offsetSteps < 0){
|
|
McuLog_info("SF_MoveMotorToZeroPosition found negative offset for Splitflap with hwId=%i. Offset set to zero.", ((SF_t*)instance)->hwId);
|
|
offsetSteps = 0;
|
|
}
|
|
|
|
if(SF_IS_RDY_TO_MOVE((SF_t*)instance)){
|
|
McuLog_warn("Tried to initialize motor of Splitflap <%i>, but already initialized => skipping", ((SF_t*)instance)->id);
|
|
return true;
|
|
}
|
|
else if(SF_IS_MOVING((SF_t*)instance)){
|
|
McuLog_warn("Tried to initialize motor of Splitflap <%i>, but it is currently moving => skipping", ((SF_t*)instance)->id);
|
|
return false;
|
|
}
|
|
|
|
if(OngoingMoveMutex_Lock(instance)){
|
|
// set state
|
|
((SF_t*)instance)->state = SF_STATE_INITIALIZATION;
|
|
|
|
// move out of sensor
|
|
while(SF_GetMagSensorAtZeroPosition((SF_t*)instance) == true){
|
|
McuULN2003_IncStep(((SF_t*)instance)->motor);
|
|
#ifdef McuLib_CONFIG_SDK_USE_FREERTOS
|
|
vTaskDelay(pdMS_TO_TICKS(20));
|
|
#else
|
|
McuWait_Waitms(20);
|
|
#endif
|
|
}
|
|
|
|
// turn until sensor is on and not reached one full rotation already (timeout)
|
|
while(SF_GetMagSensorAtZeroPosition((SF_t*)instance) == false && numStepsMoved < SPLITFLAP_STEPS_ONE_ROUND ){
|
|
McuULN2003_IncStep(((SF_t*)instance)->motor);
|
|
#ifdef McuLib_CONFIG_SDK_USE_FREERTOS
|
|
vTaskDelay(pdMS_TO_TICKS(20));
|
|
#else
|
|
McuWait_Waitms(20);
|
|
#endif
|
|
numStepsMoved++;
|
|
}
|
|
|
|
// offset after init
|
|
if(numStepsMoved < SPLITFLAP_STEPS_ONE_ROUND){
|
|
for(int i=0; i < offsetSteps; i++){
|
|
McuULN2003_IncStep(((SF_t*)instance)->motor);
|
|
#ifdef McuLib_CONFIG_SDK_USE_FREERTOS
|
|
vTaskDelay(pdMS_TO_TICKS(20));
|
|
#else
|
|
McuWait_Waitms(20);
|
|
#endif
|
|
}
|
|
}
|
|
McuULN2003_SetPos(((SF_t*)instance)->motor, 0);
|
|
McuULN2003_PowerOff(((SF_t*)instance)->motor);
|
|
|
|
OngoingMoveMutex_Unlock(instance);
|
|
} else{
|
|
// set state
|
|
((SF_t*)instance)->state = SF_STATE_NOT_READY;
|
|
McuLog_error("SF_MoveMotorToZeroPosition failed for Splitflap <%i>, could not aquire ongoing move mutex.", ((SF_t*)instance)->id);
|
|
return false;
|
|
}
|
|
|
|
// success if less than one rotation
|
|
((SF_t*)instance)->state = (numStepsMoved < SPLITFLAP_STEPS_ONE_ROUND) ? SF_STATE_READY : SF_STATE_NOT_READY;
|
|
McuLog_info("SF_MoveMotorToZeroPosition finished for Splitflap <%i>, success=%s", ((SF_t*)instance)->id, SF_IS_RDY_TO_MOVE(((SF_t*)instance)) ? "true" : "false");
|
|
|
|
return SF_IS_RDY_TO_MOVE(((SF_t*)instance));
|
|
}
|
|
|
|
void SF_MoveMotorToZeroPositionAsync(SF_Handle_t instance){
|
|
// set to state INITIALIZATION => is trigger for the task to start init
|
|
((SF_t*)instance)->state = SF_STATE_INITIALIZATION;
|
|
}
|
|
|
|
void SF_MoveSteps(SF_Handle_t instance, uint32_t steps){
|
|
if(SF_IS_RDY_TO_MOVE((SF_t*)instance)){
|
|
if(OngoingMoveMutex_Lock(instance)){
|
|
// set state
|
|
((SF_t*)instance)->state = SF_STATE_MOVING;
|
|
|
|
// run move with acceleration & deceleration
|
|
McuULN2003_AccelerationStart(((SF_t*)instance)->motor);
|
|
while(steps>0){
|
|
if(McuULN2003_StepCallback(((SF_t*)instance)->motor, true)){
|
|
steps--;
|
|
}
|
|
#ifdef McuLib_CONFIG_SDK_USE_FREERTOS
|
|
vTaskDelay(pdMS_TO_TICKS(1));
|
|
#else
|
|
McuWait_Waitms(1);
|
|
#endif
|
|
}
|
|
McuULN2003_AccelerationEnd(((SF_t*)instance)->motor);
|
|
|
|
// Power off disables all outputs of the ULN,
|
|
// required since it is possible that one is still active, which would result in the motor getting hot
|
|
// no re-init is required
|
|
McuULN2003_PowerOff(((SF_t*)instance)->motor);
|
|
|
|
// set new state
|
|
((SF_t*)instance)->state = SF_STATE_READY;
|
|
|
|
OngoingMoveMutex_Unlock(instance);
|
|
} else{
|
|
McuLog_error("SF_MoveSteps failed for Splitflap <%i>, could not aquire ongoing move mutex.", ((SF_t*)instance)->id);
|
|
}
|
|
} else{
|
|
McuLog_error("SF_MoveSteps could not be called for Splitflap <%i> because it is not initialized.", ((SF_t*)instance)->id);
|
|
}
|
|
}
|
|
|
|
bool SF_GetMagSensorAtZeroPosition(SF_Handle_t instance){
|
|
return McuGPIO_IsLow(((SF_t*)instance)->magSensor);
|
|
}
|
|
|
|
void SF_MoveToFlap(SF_Handle_t instance, Flap_t flap){
|
|
if(OngoingMoveMutex_Lock(instance)){
|
|
|
|
// get flap pos from dictonary
|
|
Position_t flapPos;
|
|
if(SF_Position_Get(flap, &flapPos) == ERR_FAILED){
|
|
McuLog_error("SF_MoveToFlap did not find the position of Flap %c", flap);
|
|
OngoingMoveMutex_Unlock(instance);
|
|
return;
|
|
}
|
|
// get current motor pos
|
|
int32_t currentPos = SF_GetMotorPosition(instance) % SPLITFLAP_STEPS_ONE_ROUND;
|
|
// calc steps to move
|
|
int32_t stepsToReachFlap = 0;
|
|
// not already there
|
|
if(flapPos != currentPos){
|
|
if(flapPos < currentPos){
|
|
stepsToReachFlap = SPLITFLAP_STEPS_ONE_ROUND-currentPos+flapPos;
|
|
}else if(flapPos > currentPos){
|
|
stepsToReachFlap = flapPos - currentPos;
|
|
}
|
|
SF_MoveSteps(instance, stepsToReachFlap);
|
|
}
|
|
|
|
OngoingMoveMutex_Unlock(instance);
|
|
}
|
|
}
|
|
|
|
void SF_MoveToFlapAsync(SF_Handle_t instance, Flap_t flap){
|
|
if(xQueueSendToBack(((SF_t*)instance)->flapQueue, &flap, pdMS_TO_TICKS(100)) != pdPASS){
|
|
McuLog_error("SF_MoveToFlapAsync: Failed to add flap to queue for Splitflap <%i>", ((SF_t*)instance)->id);
|
|
}
|
|
}
|
|
|
|
void SF_SetHardwareIdentifier(SF_Handle_t instance, HardwareIdentifier_t identifier){
|
|
((SF_t*)instance)->hwId = identifier;
|
|
#if PL_CONFIG_USE_NVMC // update hardwareid in NVMC if used
|
|
if(NVMC_SetStepperID(((SF_t*)instance)->id, identifier) != ERR_OK){
|
|
McuLog_error("SF_SetHardwareIdentifier: Failed to update hardware ID in NVMC for SplitFlap ID <%i>", ((SF_t*)instance)->id);
|
|
}
|
|
#endif
|
|
}
|
|
|
|
int32_t SF_GetMotorPosition(SF_Handle_t instance){
|
|
return McuULN2003_GetPos(((SF_t*)instance)->motor);
|
|
}
|
|
|
|
SF_State SF_GetCurrentState(SF_Handle_t instance){
|
|
return ((SF_t*)instance)->state;
|
|
}
|
|
|
|
/**********************/
|
|
/* INTERNAL FUNCTIONS */
|
|
/**********************/
|
|
/* voidpointer to instance of splitflap (SF_Handle_t) */
|
|
static void SF_Task(void *pv){
|
|
// parse parameter
|
|
SF_Handle_t instance = (SF_Handle_t)pv;
|
|
SF_t* splitflap = (SF_t*)instance;
|
|
Flap_t nextFlap = ' ';
|
|
bool initSuccess = false;
|
|
McuLog_info("Splitflap: Task for Splitflap <%i> started.", splitflap->id);
|
|
|
|
for(;;){
|
|
// delay task
|
|
vTaskDelay(pdMS_TO_TICKS(50));
|
|
|
|
// check state
|
|
switch (splitflap->state) {
|
|
// no action to be made
|
|
case SF_STATE_NOT_READY:
|
|
break; // = restart for loop
|
|
|
|
// action: start initialization
|
|
case SF_STATE_INITIALIZATION:
|
|
McuLog_info("Splitflap <%i> recognized to be in state initialization => starting zero move.", splitflap->id);
|
|
initSuccess = SF_MoveMotorToZeroPosition(instance);
|
|
McuLog_info("Splitflap <%i> finished initialization. success=%s", splitflap->id, initSuccess ? "true" : "false");
|
|
|
|
break; // = go on in the current loop
|
|
|
|
// action: check if any moves need to be made
|
|
case SF_STATE_READY:
|
|
// check if anything is in queue once (poll once, don't get item)
|
|
if(xQueuePeek(splitflap->flapQueue, &nextFlap, 0) != pdPASS){
|
|
/* failed to receive => queue empty */
|
|
continue;
|
|
}
|
|
McuLog_info("Splitflap <%i> recognized new flap '%c' in queue to move to.", splitflap->id, nextFlap);
|
|
|
|
// new flap to move to is available
|
|
// check if ongoing move
|
|
if(OngoingMoveMutex_Lock(instance)){
|
|
// now get the queue item
|
|
if(xQueueReceive(splitflap->flapQueue, &nextFlap, 0) != pdPASS){
|
|
/* failed to receive => queue empty but previously an item was there? error */
|
|
McuLog_error("Failed to receive Queue item of Splitflap <%i>.", splitflap->id);
|
|
OngoingMoveMutex_Unlock(instance);
|
|
continue; // ignore & proceed
|
|
}
|
|
|
|
//=> execute move
|
|
McuLog_info("Splitflap <%i> moving to flap '%c'.", splitflap->id, nextFlap);
|
|
SF_MoveToFlap(instance, nextFlap);
|
|
McuLog_info("Splitflap <%i> move done to flap '%c'.", splitflap->id, nextFlap);
|
|
vTaskDelay(pdMS_TO_TICKS(100)); // wait for logger to finish
|
|
|
|
OngoingMoveMutex_Unlock(instance);
|
|
}
|
|
break;
|
|
|
|
// wrong state
|
|
default:
|
|
McuLog_warn("Not implemented state in SF_Task!");
|
|
break; // = restart for loop
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
/**********************/
|
|
/* HELPERS */
|
|
/**********************/
|
|
static bool OngoingMoveMutex_Lock(SF_Handle_t instance){
|
|
/* aquire mutex */
|
|
if(xSemaphoreTakeRecursive(((SF_t*)instance)->ongoingMoveMutex, pdMS_TO_TICKS(20)) != pdTRUE){
|
|
return false; /* timeout? */
|
|
}
|
|
return true;
|
|
}
|
|
|
|
static void OngoingMoveMutex_Unlock(SF_Handle_t instance){
|
|
/* give back mutex */
|
|
if(xSemaphoreGiveRecursive(((SF_t*)instance)->ongoingMoveMutex) != pdTRUE){
|
|
/* issue */
|
|
McuLog_error("Could not give back ongoing move mutex for splitflap %i", ((SF_t*)instance)->id);
|
|
for(;;);
|
|
}
|
|
}
|
|
|