Import independent seat firmware source without build artifacts
This commit is contained in:
@@ -0,0 +1,965 @@
|
||||
/* Reusable four-axis seat action runtime for Codex-generated CAN OTA images.
|
||||
* All absolute and relative targets are calculated on the board from the
|
||||
* restored/live Hall position and immutable Base Hall range. Ordinary DIY
|
||||
* motion stays inside 5%..95% of each mechanism's travel. */
|
||||
|
||||
#define OTA_SEAT_ENDPOINT_TOLERANCE_PERCENT (5ul)
|
||||
|
||||
#define OTA_SEAT_SPEED_DEFAULT (80u)
|
||||
#define OTA_SEAT_SPEED_LOW (60u)
|
||||
#define OTA_SEAT_SPEED_MEDIUM (80u)
|
||||
#define OTA_SEAT_SPEED_HIGH (95u)
|
||||
#define OTA_SEAT_SPEED_CUSTOM(percent) ((uint8_t)(percent))
|
||||
|
||||
#define OTA_SEAT_TOWARD_FAR MOTOR_DIRECTION_TOWARD_FAR
|
||||
#define OTA_SEAT_TOWARD_NEAR MOTOR_DIRECTION_TOWARD_NEAR
|
||||
|
||||
/* Confirmed physical directions. Position zero is always the nearest end. */
|
||||
#define OTA_SEAT_SLIDE_BACKWARD OTA_SEAT_TOWARD_FAR
|
||||
#define OTA_SEAT_SLIDE_FORWARD OTA_SEAT_TOWARD_NEAR
|
||||
#define OTA_SEAT_HEIGHT_UP OTA_SEAT_TOWARD_FAR
|
||||
#define OTA_SEAT_HEIGHT_DOWN OTA_SEAT_TOWARD_NEAR
|
||||
#define OTA_SEAT_BACKREST_RECLINE_DOWN OTA_SEAT_TOWARD_FAR
|
||||
#define OTA_SEAT_BACKREST_RAISE OTA_SEAT_TOWARD_NEAR
|
||||
#define OTA_SEAT_LEGREST_RAISE OTA_SEAT_TOWARD_FAR
|
||||
#define OTA_SEAT_LEGREST_LOWER OTA_SEAT_TOWARD_NEAR
|
||||
|
||||
typedef enum
|
||||
{
|
||||
OTA_SELF_ACTION_DRIVE_TIME = 1u,
|
||||
OTA_SELF_ACTION_POSITION = 2u,
|
||||
OTA_SELF_ACTION_RELATIVE = 3u,
|
||||
OTA_SELF_ACTION_CYCLE = 4u
|
||||
} ota_self_action_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint8_t action;
|
||||
uint8_t motor;
|
||||
uint8_t direction;
|
||||
uint8_t dutyPercent;
|
||||
uint32_t value;
|
||||
uint32_t pauseMs;
|
||||
} ota_self_compose_step_t;
|
||||
|
||||
#define OTA_SEAT_STEP_TIME(axis, directionValue, durationMs, duty, pause) \
|
||||
{ OTA_SELF_ACTION_DRIVE_TIME, (axis), (directionValue), (duty), \
|
||||
(durationMs), (pause) }
|
||||
#define OTA_SEAT_STEP_TO(axis, targetX100, duty, pause) \
|
||||
{ OTA_SELF_ACTION_POSITION, (axis), MOTOR_DIRECTION_STOPPED, (duty), \
|
||||
(targetX100), (pause) }
|
||||
#define OTA_SEAT_STEP_BY(axis, directionValue, amountX100, duty, pause) \
|
||||
{ OTA_SELF_ACTION_RELATIVE, (axis), (directionValue), (duty), \
|
||||
(amountX100), (pause) }
|
||||
#define OTA_SEAT_STEP_CYCLE(axis, firstDirection, cycles, duty, endpointPauseMs) \
|
||||
{ OTA_SELF_ACTION_CYCLE, (axis), (firstDirection), (duty), \
|
||||
(cycles), (endpointPauseMs) }
|
||||
/* Explicit per-axis helpers keep generated code short and prevent remapping. */
|
||||
#define OTA_SEAT_SLIDE_FOR(direction, durationMs, duty, pauseMs) \
|
||||
OTA_SEAT_STEP_TIME(0u, direction, durationMs, duty, pauseMs)
|
||||
#define OTA_SEAT_HEIGHT_FOR(direction, durationMs, duty, pauseMs) \
|
||||
OTA_SEAT_STEP_TIME(1u, direction, durationMs, duty, pauseMs)
|
||||
#define OTA_SEAT_BACKREST_FOR(direction, durationMs, duty, pauseMs) \
|
||||
OTA_SEAT_STEP_TIME(2u, direction, durationMs, duty, pauseMs)
|
||||
#define OTA_SEAT_LEGREST_FOR(direction, durationMs, duty, pauseMs) \
|
||||
OTA_SEAT_STEP_TIME(3u, direction, durationMs, duty, pauseMs)
|
||||
|
||||
#define OTA_SEAT_SLIDE_TO(positionMmX100, duty, pauseMs) \
|
||||
OTA_SEAT_STEP_TO(0u, positionMmX100, duty, pauseMs)
|
||||
#define OTA_SEAT_HEIGHT_TO(positionMmX100, duty, pauseMs) \
|
||||
OTA_SEAT_STEP_TO(1u, positionMmX100, duty, pauseMs)
|
||||
#define OTA_SEAT_BACKREST_TO(angleDegX100, duty, pauseMs) \
|
||||
OTA_SEAT_STEP_TO(2u, angleDegX100, duty, pauseMs)
|
||||
#define OTA_SEAT_LEGREST_TO(angleDegX100, duty, pauseMs) \
|
||||
OTA_SEAT_STEP_TO(3u, angleDegX100, duty, pauseMs)
|
||||
|
||||
#define OTA_SEAT_SLIDE_BY(direction, distanceMmX100, duty, pauseMs) \
|
||||
OTA_SEAT_STEP_BY(0u, direction, distanceMmX100, duty, pauseMs)
|
||||
#define OTA_SEAT_HEIGHT_BY(direction, distanceMmX100, duty, pauseMs) \
|
||||
OTA_SEAT_STEP_BY(1u, direction, distanceMmX100, duty, pauseMs)
|
||||
#define OTA_SEAT_BACKREST_BY(direction, angleDegX100, duty, pauseMs) \
|
||||
OTA_SEAT_STEP_BY(2u, direction, angleDegX100, duty, pauseMs)
|
||||
#define OTA_SEAT_LEGREST_BY(direction, angleDegX100, duty, pauseMs) \
|
||||
OTA_SEAT_STEP_BY(3u, direction, angleDegX100, duty, pauseMs)
|
||||
|
||||
#define OTA_SEAT_SLIDE_CYCLE(firstDirection, cycles, duty, pauseMs) \
|
||||
OTA_SEAT_STEP_CYCLE(0u, firstDirection, cycles, duty, pauseMs)
|
||||
#define OTA_SEAT_HEIGHT_CYCLE(firstDirection, cycles, duty, pauseMs) \
|
||||
OTA_SEAT_STEP_CYCLE(1u, firstDirection, cycles, duty, pauseMs)
|
||||
#define OTA_SEAT_BACKREST_CYCLE(firstDirection, cycles, duty, pauseMs) \
|
||||
OTA_SEAT_STEP_CYCLE(2u, firstDirection, cycles, duty, pauseMs)
|
||||
#define OTA_SEAT_LEGREST_CYCLE(firstDirection, cycles, duty, pauseMs) \
|
||||
OTA_SEAT_STEP_CYCLE(3u, firstDirection, cycles, duty, pauseMs)
|
||||
|
||||
#include "ota_self_compose_plan.h"
|
||||
|
||||
typedef enum
|
||||
{
|
||||
OTA_SELF_PHASE_IDLE = 0u,
|
||||
OTA_SELF_PHASE_START_STEP,
|
||||
OTA_SELF_PHASE_DRIVE,
|
||||
OTA_SELF_PHASE_START_MOVE,
|
||||
OTA_SELF_PHASE_MOVE,
|
||||
OTA_SELF_PHASE_SETTLE,
|
||||
OTA_SELF_PHASE_CYCLE_PAUSE,
|
||||
OTA_SELF_PHASE_PAUSE
|
||||
} ota_self_phase_t;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
OTA_SELF_STOP_NOT_STARTED = 0u,
|
||||
OTA_SELF_STOP_COMPLETE,
|
||||
OTA_SELF_STOP_NEAR_EDGE,
|
||||
OTA_SELF_STOP_FAR_EDGE,
|
||||
OTA_SELF_STOP_STALL,
|
||||
OTA_SELF_STOP_TIMEOUT,
|
||||
OTA_SELF_STOP_DRIVER_FAULT,
|
||||
OTA_SELF_STOP_ABORTED,
|
||||
OTA_SELF_STOP_CALIBRATION_FAILED
|
||||
} ota_self_stop_reason_t;
|
||||
|
||||
static bool otaSelfDriverInitOk;
|
||||
static bool otaSelfPrepared;
|
||||
static bool otaSelfActive;
|
||||
static bool otaSelfComplete;
|
||||
static bool otaSelfDiagnosticsValid;
|
||||
static bool otaSelfPositionPersisted;
|
||||
static ota_self_phase_t otaSelfPhase;
|
||||
static ota_self_stop_reason_t otaSelfStopReason;
|
||||
static uint32_t otaSelfStepIndex;
|
||||
static uint32_t otaSelfPassIndex;
|
||||
static uint32_t otaSelfStartTicks;
|
||||
static uint32_t otaSelfPhaseStartTicks;
|
||||
static uint32_t otaSelfTargetValueX100;
|
||||
static uint32_t otaSelfCycleLegsCompleted;
|
||||
static motor_direction_t otaSelfCycleDirection;
|
||||
static uint32_t otaSelfRisingBaseline[MOTOR_MODE_COUNT];
|
||||
static uint32_t otaSelfTotalBaseline[MOTOR_MODE_COUNT];
|
||||
static uint16_t otaSelfDriverStatus;
|
||||
static uint16_t otaSelfDriverFaults;
|
||||
static bool otaSelfCanReady;
|
||||
static bool otaSelfServiceHookActive;
|
||||
|
||||
static uint32_t OtaSelf_SafeMinimumHall(uint32_t motor)
|
||||
{
|
||||
uint32_t range = motorProfiles[motor].defaultMaxHallCount;
|
||||
return (range * OTA_SEAT_ENDPOINT_TOLERANCE_PERCENT + 99ul) / 100ul;
|
||||
}
|
||||
|
||||
static uint32_t OtaSelf_SafeMaximumHall(uint32_t motor)
|
||||
{
|
||||
uint32_t range = motorProfiles[motor].defaultMaxHallCount;
|
||||
return (range * (100ul - OTA_SEAT_ENDPOINT_TOLERANCE_PERCENT)) / 100ul;
|
||||
}
|
||||
|
||||
static uint32_t OtaSelf_HallToValue(uint32_t motor, uint32_t hall)
|
||||
{
|
||||
uint32_t range = motorProfiles[motor].defaultMaxHallCount;
|
||||
uint32_t physicalMaximum = motorProfiles[motor].maxPhysicalValueX100;
|
||||
return (uint32_t)((((uint64_t)hall * physicalMaximum) + (range / 2ul)) /
|
||||
range);
|
||||
}
|
||||
|
||||
static bool OtaSelf_LivePosition(uint32_t motor, uint32_t *position)
|
||||
{
|
||||
uint32_t savedIntrStatus;
|
||||
volatile motor_runtime_t *state = &motorRuntime[motor];
|
||||
bool valid;
|
||||
|
||||
savedIntrStatus = Cy_SysLib_EnterCriticalSection();
|
||||
state->maxHallCount = motorProfiles[motor].defaultMaxHallCount;
|
||||
if(state->positionHallCount > state->maxHallCount)
|
||||
{
|
||||
state->positionHallCount = state->maxHallCount;
|
||||
}
|
||||
valid = state->positionValid;
|
||||
*position = state->positionHallCount;
|
||||
Cy_SysLib_ExitCriticalSection(savedIntrStatus);
|
||||
return valid;
|
||||
}
|
||||
|
||||
static bool OtaSelf_MotorSettled(uint32_t motor)
|
||||
{
|
||||
volatile motor_runtime_t *state = &motorRuntime[motor];
|
||||
|
||||
return (state->direction == MOTOR_DIRECTION_STOPPED) &&
|
||||
(false == state->startPending) &&
|
||||
(false == state->coastTracking) &&
|
||||
(state->pendingEvent == MOTOR_EVENT_NONE);
|
||||
}
|
||||
|
||||
static void OtaSelf_StopAllHardware(void)
|
||||
{
|
||||
uint32_t motor;
|
||||
|
||||
for(motor = 0ul; motor < MOTOR_PROFILE_COUNT; motor++)
|
||||
{
|
||||
(void)Motor_StopMode((motor_mode_t)motor);
|
||||
Cy_Tcpwm_TriggerStopOrKill(motorProfiles[motor].pwmTcpwm);
|
||||
Motor_SetPwmInputLow(&motorProfiles[motor]);
|
||||
}
|
||||
Cy_GPIO_Set(DRV_DRVOFF_PORT, DRV_DRVOFF_PIN);
|
||||
}
|
||||
|
||||
static ota_self_stop_reason_t OtaSelf_ReasonFromEvent(motor_event_t event)
|
||||
{
|
||||
switch(event)
|
||||
{
|
||||
case MOTOR_EVENT_NEAR_ENDPOINT:
|
||||
return OTA_SELF_STOP_NEAR_EDGE;
|
||||
case MOTOR_EVENT_FAR_ENDPOINT:
|
||||
return OTA_SELF_STOP_FAR_EDGE;
|
||||
case MOTOR_EVENT_STALL:
|
||||
return OTA_SELF_STOP_STALL;
|
||||
case MOTOR_EVENT_TIMEOUT:
|
||||
return OTA_SELF_STOP_TIMEOUT;
|
||||
case MOTOR_EVENT_DRIVER_FAULT:
|
||||
return OTA_SELF_STOP_DRIVER_FAULT;
|
||||
case MOTOR_EVENT_ABORTED:
|
||||
return OTA_SELF_STOP_ABORTED;
|
||||
default:
|
||||
return OTA_SELF_STOP_ABORTED;
|
||||
}
|
||||
}
|
||||
|
||||
static void OtaSelf_CaptureDiagnostics(void)
|
||||
{
|
||||
uint8_t status = 0u;
|
||||
bool statusOk = false;
|
||||
|
||||
if(false == driverFaultLatched)
|
||||
{
|
||||
statusOk = Drv8718_ReadRegRaw(DRV8718_REG_IC_STAT1, &status);
|
||||
}
|
||||
otaSelfDriverStatus = status;
|
||||
otaSelfDriverFaults = (uint16_t)(status & DRV8718_IC_STAT1_ALERT_MASK);
|
||||
otaSelfDiagnosticsValid = statusOk;
|
||||
}
|
||||
|
||||
static void OtaSelf_Finish(ota_self_stop_reason_t reason)
|
||||
{
|
||||
uint32_t motor;
|
||||
|
||||
if(false == otaSelfActive)
|
||||
{
|
||||
return;
|
||||
}
|
||||
otaSelfActive = false;
|
||||
otaSelfComplete = true;
|
||||
otaSelfStopReason = reason;
|
||||
OtaSelf_StopAllHardware();
|
||||
for(motor = 0ul; motor < MOTOR_MODE_COUNT; motor++)
|
||||
{
|
||||
(void)Motor_WaitForCoastToSettle((motor_mode_t)motor);
|
||||
}
|
||||
if(false == otaSelfPositionPersisted)
|
||||
{
|
||||
otaSelfPositionPersisted = OtaSeat_SavePersistentState();
|
||||
}
|
||||
OtaSelf_CaptureDiagnostics();
|
||||
if(false != driverFaultLatched)
|
||||
{
|
||||
otaSelfStopReason = OTA_SELF_STOP_DRIVER_FAULT;
|
||||
}
|
||||
}
|
||||
|
||||
static void OtaSelf_Abort(void)
|
||||
{
|
||||
if(false != otaSelfActive)
|
||||
{
|
||||
OtaSelf_Finish(OTA_SELF_STOP_ABORTED);
|
||||
}
|
||||
else
|
||||
{
|
||||
OtaSelf_StopAllHardware();
|
||||
}
|
||||
}
|
||||
|
||||
static uint8_t OtaSelf_ResolvedDuty(const ota_self_compose_step_t *step)
|
||||
{
|
||||
return (0u == step->dutyPercent) ?
|
||||
OTA_SEAT_SPEED_DEFAULT : step->dutyPercent;
|
||||
}
|
||||
|
||||
static bool OtaSelf_Start(void)
|
||||
{
|
||||
uint32_t motor;
|
||||
uint32_t savedIntrStatus;
|
||||
motor_safety_options_t safety = { false, false };
|
||||
|
||||
if((false != otaSelfActive) || (false == otaSelfDriverInitOk) ||
|
||||
(false != driverFaultLatched))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
OtaSelf_StopAllHardware();
|
||||
Cy_GPIO_Clr(DRV_DRVOFF_PORT, DRV_DRVOFF_PIN);
|
||||
Cy_SysTick_DelayInUs(100ul);
|
||||
for(motor = 0ul; motor < MOTOR_PROFILE_COUNT; motor++)
|
||||
{
|
||||
if((MOTOR_API_SUCCESS !=
|
||||
Motor_SetSafetyOptions((motor_mode_t)motor, &safety)) ||
|
||||
(MOTOR_API_SUCCESS != Motor_DisableSpeedControl((motor_mode_t)motor)) ||
|
||||
(MOTOR_API_SUCCESS != Motor_SetDutyPercent(
|
||||
(motor_mode_t)motor, OTA_SEAT_SPEED_DEFAULT)))
|
||||
{
|
||||
OtaSelf_StopAllHardware();
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
savedIntrStatus = Cy_SysLib_EnterCriticalSection();
|
||||
for(motor = 0ul; motor < MOTOR_PROFILE_COUNT; motor++)
|
||||
{
|
||||
otaSelfRisingBaseline[motor] = motorRuntime[motor].rawRisingEdges;
|
||||
otaSelfTotalBaseline[motor] = motorRuntime[motor].rawTotalEdges;
|
||||
}
|
||||
Cy_SysLib_ExitCriticalSection(savedIntrStatus);
|
||||
|
||||
otaSelfStepIndex = 0ul;
|
||||
otaSelfPassIndex = 0ul;
|
||||
otaSelfStartTicks = hallTimebaseTicks;
|
||||
otaSelfPhaseStartTicks = hallTimebaseTicks;
|
||||
otaSelfTargetValueX100 = 0ul;
|
||||
otaSelfCycleLegsCompleted = 0ul;
|
||||
otaSelfCycleDirection = MOTOR_DIRECTION_STOPPED;
|
||||
otaSelfDriverStatus = 0u;
|
||||
otaSelfDriverFaults = 0u;
|
||||
otaSelfDiagnosticsValid = false;
|
||||
otaSelfPositionPersisted = true;
|
||||
otaSelfStopReason = OTA_SELF_STOP_NOT_STARTED;
|
||||
otaSelfComplete = false;
|
||||
otaSelfPrepared = true;
|
||||
otaSelfPhase = OTA_SELF_PHASE_START_STEP;
|
||||
otaSelfActive = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool OtaSelf_StartSafeTimedLeg(const ota_self_compose_step_t *step)
|
||||
{
|
||||
uint32_t position;
|
||||
uint32_t nearHall = OtaSelf_SafeMinimumHall(step->motor);
|
||||
uint32_t farHall = OtaSelf_SafeMaximumHall(step->motor);
|
||||
|
||||
if(false == OtaSelf_LivePosition(step->motor, &position))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
otaSelfCycleDirection = (motor_direction_t)step->direction;
|
||||
if((otaSelfCycleDirection == MOTOR_DIRECTION_TOWARD_FAR) &&
|
||||
(position >= farHall))
|
||||
{
|
||||
otaSelfCycleDirection = MOTOR_DIRECTION_TOWARD_NEAR;
|
||||
}
|
||||
else if((otaSelfCycleDirection == MOTOR_DIRECTION_TOWARD_NEAR) &&
|
||||
(position <= nearHall))
|
||||
{
|
||||
otaSelfCycleDirection = MOTOR_DIRECTION_TOWARD_FAR;
|
||||
}
|
||||
return MOTOR_API_SUCCESS == Motor_StartDirectionInternal(
|
||||
(motor_mode_t)step->motor, otaSelfCycleDirection,
|
||||
false, 0ul, false, true,
|
||||
MOTOR_DIRECTION_STOPPED, false);
|
||||
}
|
||||
|
||||
static bool OtaSelf_TargetFromBoard(const ota_self_compose_step_t *step,
|
||||
uint32_t *targetValueX100)
|
||||
{
|
||||
motor_position_info_t info;
|
||||
uint32_t safeMinimum = OtaSelf_HallToValue(
|
||||
step->motor, OtaSelf_SafeMinimumHall(step->motor));
|
||||
uint32_t safeMaximum = OtaSelf_HallToValue(
|
||||
step->motor, OtaSelf_SafeMaximumHall(step->motor));
|
||||
uint32_t current;
|
||||
|
||||
if(step->action == OTA_SELF_ACTION_POSITION)
|
||||
{
|
||||
*targetValueX100 = step->value;
|
||||
if(*targetValueX100 < safeMinimum) *targetValueX100 = safeMinimum;
|
||||
if(*targetValueX100 > safeMaximum) *targetValueX100 = safeMaximum;
|
||||
return true;
|
||||
}
|
||||
if(step->action == OTA_SELF_ACTION_CYCLE)
|
||||
{
|
||||
*targetValueX100 =
|
||||
(otaSelfCycleDirection == MOTOR_DIRECTION_TOWARD_FAR) ?
|
||||
safeMaximum : safeMinimum;
|
||||
return true;
|
||||
}
|
||||
if((step->action != OTA_SELF_ACTION_RELATIVE) ||
|
||||
(MOTOR_API_SUCCESS != Motor_GetPositionInfo(
|
||||
(motor_mode_t)step->motor, &info)) ||
|
||||
(false == info.positionValid))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
current = info.valueX100;
|
||||
if(step->direction == MOTOR_DIRECTION_TOWARD_FAR)
|
||||
{
|
||||
*targetValueX100 =
|
||||
(step->value >= (safeMaximum - ((current > safeMaximum) ?
|
||||
safeMaximum : current))) ? safeMaximum : (current + step->value);
|
||||
}
|
||||
else
|
||||
{
|
||||
*targetValueX100 =
|
||||
(step->value >= ((current > safeMinimum) ?
|
||||
(current - safeMinimum) : 0ul)) ? safeMinimum :
|
||||
(current - step->value);
|
||||
}
|
||||
if(*targetValueX100 < safeMinimum)
|
||||
{
|
||||
*targetValueX100 = safeMinimum;
|
||||
}
|
||||
else if(*targetValueX100 > safeMaximum)
|
||||
{
|
||||
*targetValueX100 = safeMaximum;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool OtaSelf_AtSafeBoundary(uint32_t motor,
|
||||
motor_direction_t direction)
|
||||
{
|
||||
volatile motor_runtime_t *state = &motorRuntime[motor];
|
||||
uint32_t position;
|
||||
uint32_t range;
|
||||
uint32_t savedIntrStatus = Cy_SysLib_EnterCriticalSection();
|
||||
|
||||
position = state->positionHallCount;
|
||||
range = state->maxHallCount;
|
||||
Cy_SysLib_ExitCriticalSection(savedIntrStatus);
|
||||
if(0ul == range)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if(direction == MOTOR_DIRECTION_TOWARD_NEAR)
|
||||
{
|
||||
return position <= OtaSelf_SafeMinimumHall(motor);
|
||||
}
|
||||
return position >= OtaSelf_SafeMaximumHall(motor);
|
||||
}
|
||||
|
||||
static bool OtaSelf_ReverseTimedAtBoundary(
|
||||
const ota_self_compose_step_t *step)
|
||||
{
|
||||
volatile motor_runtime_t *state = &motorRuntime[step->motor];
|
||||
motor_direction_t direction = state->direction;
|
||||
motor_direction_t reverse;
|
||||
|
||||
if((direction == MOTOR_DIRECTION_STOPPED) ||
|
||||
(false == OtaSelf_AtSafeBoundary(step->motor, direction)))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
reverse = (direction == MOTOR_DIRECTION_TOWARD_FAR) ?
|
||||
MOTOR_DIRECTION_TOWARD_NEAR : MOTOR_DIRECTION_TOWARD_FAR;
|
||||
otaSelfCycleDirection = reverse;
|
||||
return MOTOR_API_SUCCESS == Motor_StartDirectionInternal(
|
||||
(motor_mode_t)step->motor, reverse, false, 0ul, false, true,
|
||||
direction, true);
|
||||
}
|
||||
|
||||
static bool OtaSelf_TimedBoundaryEvent(
|
||||
const ota_self_compose_step_t *step,
|
||||
motor_event_t event)
|
||||
{
|
||||
motor_direction_t direction = otaSelfCycleDirection;
|
||||
motor_direction_t reverse;
|
||||
|
||||
if((event != MOTOR_EVENT_NEAR_ENDPOINT) &&
|
||||
(event != MOTOR_EVENT_FAR_ENDPOINT))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
reverse = (direction == MOTOR_DIRECTION_TOWARD_FAR) ?
|
||||
MOTOR_DIRECTION_TOWARD_NEAR : MOTOR_DIRECTION_TOWARD_FAR;
|
||||
otaSelfCycleDirection = reverse;
|
||||
return MOTOR_API_SUCCESS == Motor_StartDirectionInternal(
|
||||
(motor_mode_t)step->motor, reverse, false, 0ul, false, true,
|
||||
direction, true);
|
||||
}
|
||||
|
||||
static void OtaSelf_CompleteStep(void)
|
||||
{
|
||||
const ota_self_compose_step_t *step =
|
||||
&otaSelfComposePlan[otaSelfStepIndex];
|
||||
|
||||
otaSelfPositionPersisted = OtaSeat_SavePersistentState();
|
||||
otaSelfPhaseStartTicks = hallTimebaseTicks;
|
||||
otaSelfPhase = (0ul == step->pauseMs) ?
|
||||
OTA_SELF_PHASE_START_STEP : OTA_SELF_PHASE_PAUSE;
|
||||
if(0ul == step->pauseMs)
|
||||
{
|
||||
otaSelfStepIndex++;
|
||||
}
|
||||
}
|
||||
|
||||
static void OtaSelf_AdvanceAfterPause(void)
|
||||
{
|
||||
otaSelfStepIndex++;
|
||||
otaSelfPhase = OTA_SELF_PHASE_START_STEP;
|
||||
}
|
||||
|
||||
static void OtaSelf_CompleteCycleLeg(const ota_self_compose_step_t *step)
|
||||
{
|
||||
otaSelfCycleLegsCompleted++;
|
||||
if((0ul != step->value) &&
|
||||
(otaSelfCycleLegsCompleted >= (step->value * 2ul)))
|
||||
{
|
||||
OtaSelf_CompleteStep();
|
||||
return;
|
||||
}
|
||||
|
||||
otaSelfCycleDirection =
|
||||
(otaSelfCycleDirection == MOTOR_DIRECTION_TOWARD_FAR) ?
|
||||
MOTOR_DIRECTION_TOWARD_NEAR : MOTOR_DIRECTION_TOWARD_FAR;
|
||||
otaSelfPhaseStartTicks = hallTimebaseTicks;
|
||||
otaSelfPhase = (0ul == step->pauseMs) ?
|
||||
OTA_SELF_PHASE_START_MOVE : OTA_SELF_PHASE_CYCLE_PAUSE;
|
||||
}
|
||||
|
||||
static bool OtaSelf_AdvancePassIfNeeded(void)
|
||||
{
|
||||
if(otaSelfStepIndex < OTA_SELF_COMPOSE_STEP_COUNT)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
otaSelfPassIndex++;
|
||||
if((0u != OTA_SELF_COMPOSE_PROGRAM_REPEAT) &&
|
||||
(otaSelfPassIndex >= OTA_SELF_COMPOSE_PROGRAM_REPEAT))
|
||||
{
|
||||
OtaSelf_Finish(OTA_SELF_STOP_COMPLETE);
|
||||
return false;
|
||||
}
|
||||
otaSelfStepIndex = 0ul;
|
||||
return true;
|
||||
}
|
||||
|
||||
static void OtaSelf_Service(void)
|
||||
{
|
||||
const ota_self_compose_step_t *step;
|
||||
volatile motor_runtime_t *state;
|
||||
uint32_t elapsed;
|
||||
motor_event_t event;
|
||||
|
||||
if(false == otaSelfActive)
|
||||
{
|
||||
return;
|
||||
}
|
||||
if(false != driverFaultLatched)
|
||||
{
|
||||
OtaSelf_Finish(OTA_SELF_STOP_DRIVER_FAULT);
|
||||
return;
|
||||
}
|
||||
elapsed = MOTOR_TICKS_TO_MS(hallTimebaseTicks - otaSelfStartTicks);
|
||||
if((0ul != OTA_SELF_COMPOSE_MAX_RUNTIME_MS) &&
|
||||
(elapsed >= OTA_SELF_COMPOSE_MAX_RUNTIME_MS))
|
||||
{
|
||||
OtaSelf_Finish(OTA_SELF_STOP_COMPLETE);
|
||||
return;
|
||||
}
|
||||
if(false == OtaSelf_AdvancePassIfNeeded())
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
step = &otaSelfComposePlan[otaSelfStepIndex];
|
||||
state = &motorRuntime[step->motor];
|
||||
switch(otaSelfPhase)
|
||||
{
|
||||
case OTA_SELF_PHASE_START_STEP:
|
||||
if(MOTOR_API_SUCCESS != Motor_SetDutyPercent(
|
||||
(motor_mode_t)step->motor, OtaSelf_ResolvedDuty(step)))
|
||||
{
|
||||
OtaSelf_Finish(OTA_SELF_STOP_ABORTED);
|
||||
return;
|
||||
}
|
||||
otaSelfPositionPersisted = false;
|
||||
otaSelfCycleLegsCompleted = 0ul;
|
||||
otaSelfCycleDirection = (motor_direction_t)step->direction;
|
||||
if(step->action == OTA_SELF_ACTION_DRIVE_TIME)
|
||||
{
|
||||
if(false == OtaSelf_StartSafeTimedLeg(step))
|
||||
{
|
||||
OtaSelf_Finish(OTA_SELF_STOP_ABORTED);
|
||||
return;
|
||||
}
|
||||
otaSelfPhaseStartTicks = hallTimebaseTicks;
|
||||
otaSelfPhase = OTA_SELF_PHASE_DRIVE;
|
||||
}
|
||||
else if(false == state->positionValid)
|
||||
{
|
||||
/* Ordinary DIY images never seek a physical endpoint. Position
|
||||
* must come from the board's persisted state. */
|
||||
OtaSelf_Finish(OTA_SELF_STOP_ABORTED);
|
||||
return;
|
||||
}
|
||||
else
|
||||
{
|
||||
otaSelfPhase = OTA_SELF_PHASE_START_MOVE;
|
||||
}
|
||||
break;
|
||||
|
||||
case OTA_SELF_PHASE_DRIVE:
|
||||
event = state->lastEvent;
|
||||
if((event != MOTOR_EVENT_NONE) && OtaSelf_MotorSettled(step->motor))
|
||||
{
|
||||
if(false != OtaSelf_TimedBoundaryEvent(step, event))
|
||||
{
|
||||
state->lastEvent = MOTOR_EVENT_NONE;
|
||||
}
|
||||
else
|
||||
{
|
||||
OtaSelf_Finish(OtaSelf_ReasonFromEvent(event));
|
||||
}
|
||||
}
|
||||
else if(false == OtaSelf_ReverseTimedAtBoundary(step))
|
||||
{
|
||||
OtaSelf_Finish(OTA_SELF_STOP_ABORTED);
|
||||
}
|
||||
else if(MOTOR_TICKS_TO_MS(hallTimebaseTicks - otaSelfPhaseStartTicks) >=
|
||||
step->value)
|
||||
{
|
||||
if(MOTOR_API_SUCCESS != Motor_StopMode((motor_mode_t)step->motor))
|
||||
{
|
||||
OtaSelf_Finish(OTA_SELF_STOP_DRIVER_FAULT);
|
||||
return;
|
||||
}
|
||||
otaSelfPhase = OTA_SELF_PHASE_SETTLE;
|
||||
}
|
||||
break;
|
||||
|
||||
case OTA_SELF_PHASE_START_MOVE:
|
||||
if(false == OtaSelf_TargetFromBoard(step, &otaSelfTargetValueX100))
|
||||
{
|
||||
OtaSelf_Finish(OTA_SELF_STOP_ABORTED);
|
||||
return;
|
||||
}
|
||||
if(MOTOR_API_SUCCESS != Motor_StartMoveToValue(
|
||||
(motor_mode_t)step->motor, otaSelfTargetValueX100))
|
||||
{
|
||||
OtaSelf_Finish(OTA_SELF_STOP_ABORTED);
|
||||
return;
|
||||
}
|
||||
otaSelfPhase = OTA_SELF_PHASE_MOVE;
|
||||
break;
|
||||
|
||||
case OTA_SELF_PHASE_MOVE:
|
||||
event = state->lastEvent;
|
||||
if((event != MOTOR_EVENT_NONE) && OtaSelf_MotorSettled(step->motor))
|
||||
{
|
||||
if(step->action == OTA_SELF_ACTION_CYCLE)
|
||||
{
|
||||
if(event == MOTOR_EVENT_TARGET_REACHED)
|
||||
{
|
||||
OtaSelf_CompleteCycleLeg(step);
|
||||
}
|
||||
else
|
||||
{
|
||||
OtaSelf_Finish(OtaSelf_ReasonFromEvent(event));
|
||||
}
|
||||
}
|
||||
else if(event == MOTOR_EVENT_TARGET_REACHED)
|
||||
{
|
||||
OtaSelf_CompleteStep();
|
||||
}
|
||||
else
|
||||
{
|
||||
OtaSelf_Finish(OtaSelf_ReasonFromEvent(event));
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case OTA_SELF_PHASE_SETTLE:
|
||||
if(false != OtaSelf_MotorSettled(step->motor))
|
||||
{
|
||||
OtaSelf_CompleteStep();
|
||||
}
|
||||
break;
|
||||
|
||||
case OTA_SELF_PHASE_CYCLE_PAUSE:
|
||||
if(MOTOR_TICKS_TO_MS(hallTimebaseTicks - otaSelfPhaseStartTicks) >=
|
||||
step->pauseMs)
|
||||
{
|
||||
otaSelfPhase = OTA_SELF_PHASE_START_MOVE;
|
||||
}
|
||||
break;
|
||||
|
||||
case OTA_SELF_PHASE_PAUSE:
|
||||
if(MOTOR_TICKS_TO_MS(hallTimebaseTicks - otaSelfPhaseStartTicks) >=
|
||||
step->pauseMs)
|
||||
{
|
||||
OtaSelf_AdvanceAfterPause();
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
OtaSelf_Finish(OTA_SELF_STOP_ABORTED);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
static uint32_t OtaSelf_CurrentMode(void)
|
||||
{
|
||||
if(otaSelfStepIndex < OTA_SELF_COMPOSE_STEP_COUNT)
|
||||
{
|
||||
return otaSelfComposePlan[otaSelfStepIndex].motor;
|
||||
}
|
||||
return 0ul;
|
||||
}
|
||||
|
||||
static uint32_t OtaSelf_CurrentDirection(void)
|
||||
{
|
||||
uint32_t motor = OtaSelf_CurrentMode();
|
||||
return (uint32_t)motorRuntime[motor].direction;
|
||||
}
|
||||
|
||||
static uint32_t OtaSelf_StateValue(void)
|
||||
{
|
||||
uint32_t value =
|
||||
((uint32_t)otaSelfStopReason << OTA_TEST_STATE_STOP_REASON_SHIFT) |
|
||||
(OtaSelf_CurrentMode() << OTA_TEST_STATE_MODE_SHIFT) |
|
||||
(OtaSelf_CurrentDirection() << OTA_TEST_STATE_DIRECTION_SHIFT);
|
||||
|
||||
if(false != otaSelfActive) value |= OTA_TEST_STATE_ACTIVE;
|
||||
if(false != otaSelfComplete) value |= OTA_TEST_STATE_COMPLETE;
|
||||
if(false != driverFaultLatched) value |= OTA_TEST_STATE_DRIVER_FAULT;
|
||||
if(false != otaSelfPrepared) value |= OTA_TEST_STATE_PREPARED;
|
||||
if((false != otaSelfComplete) &&
|
||||
(otaSelfStopReason == OTA_SELF_STOP_COMPLETE))
|
||||
{
|
||||
value |= OTA_TEST_STATE_TIMING_VALID;
|
||||
}
|
||||
if(false != otaSelfDiagnosticsValid)
|
||||
{
|
||||
value |= OTA_TEST_STATE_DIAGNOSTICS_VALID;
|
||||
}
|
||||
return value;
|
||||
}
|
||||
|
||||
static uint32_t OtaSelf_EdgeDelta(bool rising)
|
||||
{
|
||||
uint32_t motor;
|
||||
uint32_t total = 0ul;
|
||||
uint32_t savedIntrStatus = Cy_SysLib_EnterCriticalSection();
|
||||
|
||||
for(motor = 0ul; motor < MOTOR_PROFILE_COUNT; motor++)
|
||||
{
|
||||
total += rising ?
|
||||
(motorRuntime[motor].rawRisingEdges - otaSelfRisingBaseline[motor]) :
|
||||
(motorRuntime[motor].rawTotalEdges - otaSelfTotalBaseline[motor]);
|
||||
}
|
||||
Cy_SysLib_ExitCriticalSection(savedIntrStatus);
|
||||
return total;
|
||||
}
|
||||
|
||||
static uint32_t OtaSelf_InfoValue(uint8_t selector)
|
||||
{
|
||||
uint32_t motor = OtaSelf_CurrentMode();
|
||||
|
||||
switch((ota_test_info_selector_t)selector)
|
||||
{
|
||||
case OTA_TEST_INFO_STATE:
|
||||
return OtaSelf_StateValue();
|
||||
case OTA_TEST_INFO_RISING_EDGES:
|
||||
return OtaSelf_EdgeDelta(true);
|
||||
case OTA_TEST_INFO_TOTAL_EDGES:
|
||||
return OtaSelf_EdgeDelta(false);
|
||||
case OTA_TEST_INFO_ACTIVE_MS:
|
||||
return MOTOR_TICKS_TO_MS(hallTimebaseTicks - otaSelfStartTicks);
|
||||
case OTA_TEST_INFO_SAFETY_WINDOW_PULSES:
|
||||
return motorRuntime[motor].recentWindowPulses;
|
||||
case OTA_TEST_INFO_DRIVER_DIAGNOSTICS:
|
||||
return ((uint32_t)otaSelfDriverFaults << 16u) |
|
||||
(uint32_t)otaSelfDriverStatus;
|
||||
case OTA_TEST_INFO_CALIBRATION_HALL:
|
||||
return 0ul;
|
||||
default:
|
||||
return 0ul;
|
||||
}
|
||||
}
|
||||
|
||||
static void OtaSelf_Respond(const ota_can_control_frame_t *frame,
|
||||
ota_status_t status,
|
||||
uint32_t value)
|
||||
{
|
||||
(void)OtaCan_QueueResponse(frame->data[1], frame->data[2],
|
||||
(uint8_t)status, value);
|
||||
}
|
||||
|
||||
static uint32_t OtaSelf_ReadLe32(const uint8_t *data)
|
||||
{
|
||||
return ((uint32_t)data[0]) |
|
||||
((uint32_t)data[1] << 8u) |
|
||||
((uint32_t)data[2] << 16u) |
|
||||
((uint32_t)data[3] << 24u);
|
||||
}
|
||||
|
||||
static void OtaSelf_EnterBootloader(const ota_can_control_frame_t *frame)
|
||||
{
|
||||
uint32_t waitMs;
|
||||
|
||||
SeatDemoCompat_PrepareForBootloader();
|
||||
if(false != otaSelfActive)
|
||||
{
|
||||
otaSelfActive = false;
|
||||
otaSelfComplete = true;
|
||||
otaSelfStopReason = OTA_SELF_STOP_ABORTED;
|
||||
}
|
||||
OtaSelf_StopAllHardware();
|
||||
OtaSelf_Respond(frame, OTA_STATUS_OK, OtaSelf_StateValue());
|
||||
for(waitMs = 0ul; waitMs < 100ul; waitMs++)
|
||||
{
|
||||
OtaCan_Service();
|
||||
if(false != OtaCan_TxIdle()) break;
|
||||
Cy_SysTick_DelayInUs(1000ul);
|
||||
}
|
||||
Ota_EnterBootloader();
|
||||
}
|
||||
|
||||
static void OtaSelf_HandleControl(const ota_can_control_frame_t *frame)
|
||||
{
|
||||
uint8_t opcode = frame->data[2];
|
||||
uint32_t argument;
|
||||
uint32_t targetPermille;
|
||||
uint8_t dutyPercent;
|
||||
uint8_t status;
|
||||
|
||||
if(frame->data[0] != OTA_CONTROL_MAGIC)
|
||||
{
|
||||
OtaSelf_Respond(frame, OTA_STATUS_BAD_MAGIC, 0ul);
|
||||
return;
|
||||
}
|
||||
|
||||
if(false != OtaSeat_HandleInfoControl(frame))
|
||||
{
|
||||
return;
|
||||
}
|
||||
switch((ota_opcode_t)opcode)
|
||||
{
|
||||
case OTA_OPCODE_SEAT_MOVE:
|
||||
case OTA_OPCODE_SEAT_MOVE_WITH_DUTY:
|
||||
argument = OtaSelf_ReadLe32(&frame->data[4]);
|
||||
targetPermille = argument;
|
||||
dutyPercent = 0u;
|
||||
if(opcode == (uint8_t)OTA_OPCODE_SEAT_MOVE_WITH_DUTY)
|
||||
{
|
||||
targetPermille = argument & 0x0000FFFFul;
|
||||
dutyPercent = (uint8_t)((argument >> 16u) & 0xFFul);
|
||||
if((argument & 0xFF000000ul) != 0ul)
|
||||
{
|
||||
OtaSelf_Respond(frame, OTA_STATUS_BAD_ARGUMENT, argument);
|
||||
break;
|
||||
}
|
||||
}
|
||||
status = SeatDemoCompat_StartNativeMove(
|
||||
frame->data[1], frame->data[3], targetPermille, dutyPercent);
|
||||
OtaSelf_Respond(frame, (ota_status_t)status,
|
||||
OtaSeat_JobStateValue());
|
||||
break;
|
||||
case OTA_OPCODE_SEAT_HEARTBEAT:
|
||||
argument = OtaSelf_ReadLe32(&frame->data[4]);
|
||||
if((argument != 0ul) ||
|
||||
((frame->data[3] >= MOTOR_PROFILE_COUNT) &&
|
||||
(frame->data[3] != OTA_SEAT_AXIS_ALL)))
|
||||
{
|
||||
OtaSelf_Respond(frame, OTA_STATUS_BAD_ARGUMENT, argument);
|
||||
}
|
||||
else
|
||||
{
|
||||
/* Demo-owned moves are bounded by their target and timeout, so
|
||||
* 0x65 is accepted for wire compatibility but owns no lease. */
|
||||
OtaSelf_Respond(frame, OTA_STATUS_OK, OtaSeat_JobStateValue());
|
||||
}
|
||||
break;
|
||||
case OTA_OPCODE_SEAT_STOP:
|
||||
OtaSelf_Abort();
|
||||
SeatDemoCompat_StopAllFromNative();
|
||||
OtaSelf_Respond(frame, OTA_STATUS_OK, OtaSeat_JobStateValue());
|
||||
break;
|
||||
case OTA_OPCODE_TEST_INFO:
|
||||
if(frame->data[3] > OTA_TEST_INFO_CALIBRATION_HALL)
|
||||
{
|
||||
OtaSelf_Respond(frame, OTA_STATUS_BAD_ARGUMENT, frame->data[3]);
|
||||
}
|
||||
else
|
||||
{
|
||||
OtaSelf_Respond(frame, OTA_STATUS_OK,
|
||||
OtaSelf_InfoValue(frame->data[3]));
|
||||
}
|
||||
break;
|
||||
case OTA_OPCODE_TEST_START:
|
||||
SeatDemoCompat_StopAllFromNative();
|
||||
OtaSelf_Respond(frame,
|
||||
OtaSelf_Start() ? OTA_STATUS_OK : OTA_STATUS_BAD_STATE,
|
||||
OtaSelf_StateValue());
|
||||
break;
|
||||
case OTA_OPCODE_TEST_STOP:
|
||||
SeatDemoCompat_StopAllFromNative();
|
||||
OtaSelf_Abort();
|
||||
OtaSelf_Respond(frame, OTA_STATUS_OK, OtaSelf_StateValue());
|
||||
break;
|
||||
case OTA_OPCODE_ENTER_BOOTLOADER:
|
||||
OtaSelf_EnterBootloader(frame);
|
||||
break;
|
||||
default:
|
||||
OtaSelf_Respond(frame, OTA_STATUS_BAD_OPCODE, opcode);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* Keep CAN position/status queries and STOP responsive from blocking motor
|
||||
* helper paths. Communication health never owns the action lifetime. */
|
||||
static void OtaSelfCompose_ServiceHook(void)
|
||||
{
|
||||
ota_can_control_frame_t frame;
|
||||
|
||||
if((false == otaSelfCanReady) || (false != otaSelfServiceHookActive))
|
||||
{
|
||||
return;
|
||||
}
|
||||
otaSelfServiceHookActive = true;
|
||||
OtaCan_Service();
|
||||
while(false != OtaCan_PopControl(&frame))
|
||||
{
|
||||
OtaSelf_HandleControl(&frame);
|
||||
OtaCan_Service();
|
||||
}
|
||||
otaSelfServiceHookActive = false;
|
||||
}
|
||||
|
||||
static int OtaSelfCompose_Run(bool driverInitOk)
|
||||
{
|
||||
otaSelfDriverInitOk = driverInitOk;
|
||||
otaSelfPrepared = driverInitOk;
|
||||
OtaSeat_RestorePersistentState();
|
||||
otaSelfActive = false;
|
||||
otaSelfComplete = false;
|
||||
otaSelfDiagnosticsValid = false;
|
||||
otaSelfPhase = OTA_SELF_PHASE_IDLE;
|
||||
otaSelfStopReason = OTA_SELF_STOP_NOT_STARTED;
|
||||
otaSelfStepIndex = 0ul;
|
||||
otaSelfPassIndex = 0ul;
|
||||
otaSelfStartTicks = hallTimebaseTicks;
|
||||
otaSelfCanReady = false;
|
||||
otaSelfServiceHookActive = false;
|
||||
OtaSelf_StopAllHardware();
|
||||
OtaCan_DebugStart();
|
||||
if(false == OtaCan_Init())
|
||||
{
|
||||
for(;;) OtaSelf_StopAllHardware();
|
||||
}
|
||||
otaSelfCanReady = true;
|
||||
SeatDemoCompat_Init();
|
||||
|
||||
for(;;)
|
||||
{
|
||||
Motor_ServiceAll();
|
||||
OtaSelf_Service();
|
||||
__WFI();
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user