966 lines
30 KiB
C++
966 lines
30 KiB
C++
/* 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();
|
|
}
|
|
}
|