1362 lines
45 KiB
C++
1362 lines
45 KiB
C++
/* Dedicated four-axis calibration/homing firmware. The established motor
|
|
* implementation performs each blocking endpoint seek; the service hook below
|
|
* keeps CAN telemetry, explicit STOP and phase progress alive inside those
|
|
* blocking loops. Communication activity never owns the job lifetime: once a
|
|
* calibration/home request is accepted, CAN silence, query failure, RX overflow
|
|
* and Bus-Off do not abort motion. Only explicit STOP, a real driver fault or
|
|
* the motor safety/endpoint result can end the job. */
|
|
|
|
#define OTA_CALIBRATION_RETURN_TOLERANCE_PERCENT (20ul)
|
|
#define OTA_CALIBRATION_MINIMUM_PULSES (32ul)
|
|
#define OTA_CALIBRATION_TRACE_MAGIC (0x43414C54ul)
|
|
#define OTA_CALIBRATION_TRACE_CAPACITY (16u)
|
|
#define OTA_INTERACTIVE_MOVE_LEASE_MS (1500ul)
|
|
#define OTA_INTERACTIVE_MOVE_SAVE_DELAY_MS (750ul)
|
|
#define OTA_INTERACTIVE_MOVE_TARGET_MAX (1000ul)
|
|
#define OTA_INTERACTIVE_MOVE_TARGET_MASK (0x0000FFFFul)
|
|
#define OTA_INTERACTIVE_MOVE_DUTY_SHIFT (16u)
|
|
#define OTA_INTERACTIVE_MOVE_RESERVED_MASK (0xFF000000ul)
|
|
#define OTA_DYNAMIC_DEFAULT_DURATION_SECONDS (60ul)
|
|
#define OTA_DYNAMIC_MIN_DURATION_SECONDS (5ul)
|
|
#define OTA_DYNAMIC_MAX_DURATION_SECONDS (300ul)
|
|
#define OTA_DYNAMIC_DWELL_MS (320ul)
|
|
#define OTA_DYNAMIC_MIN_TARGET_PERMILLE (50l)
|
|
#define OTA_DYNAMIC_MAX_TARGET_PERMILLE (950l)
|
|
|
|
typedef struct
|
|
{
|
|
uint32_t sequence;
|
|
uint32_t event;
|
|
uint32_t tick;
|
|
uint32_t jobState;
|
|
uint32_t position;
|
|
uint32_t range;
|
|
} ota_calibration_trace_event_t;
|
|
|
|
typedef struct
|
|
{
|
|
uint32_t magic;
|
|
volatile uint32_t nextSequence;
|
|
volatile ota_calibration_trace_event_t
|
|
events[OTA_CALIBRATION_TRACE_CAPACITY];
|
|
} ota_calibration_trace_log_t;
|
|
|
|
/* Bounded board-side journal readable with CMSIS-DAP after a failure. Only
|
|
* task/axis/phase transitions are recorded; CAN frames and Hall samples are
|
|
* deliberately excluded. */
|
|
__root volatile ota_calibration_trace_log_t g_otaCalibrationTrace;
|
|
|
|
typedef enum
|
|
{
|
|
OTA_CALIBRATION_ERROR_NONE = 0u,
|
|
OTA_CALIBRATION_ERROR_MOTOR = 1u,
|
|
OTA_CALIBRATION_ERROR_RANGE = 2u,
|
|
OTA_CALIBRATION_ERROR_RETURN = 3u,
|
|
OTA_CALIBRATION_ERROR_STORAGE = 4u,
|
|
OTA_CALIBRATION_ERROR_ABORTED = 5u,
|
|
OTA_CALIBRATION_ERROR_LEASE = 6u,
|
|
OTA_CALIBRATION_ERROR_CAN = 7u,
|
|
OTA_CALIBRATION_ERROR_DRIVER = 8u
|
|
} ota_calibration_error_t;
|
|
|
|
static bool otaCalibrationCanReady;
|
|
static bool otaCalibrationHookActive;
|
|
static bool otaCalibrationAbortRequested;
|
|
static bool otaCalibrationPendingBootloader;
|
|
static ota_seat_job_type_t otaCalibrationPendingJob = OTA_SEAT_JOB_NONE;
|
|
static uint8_t otaCalibrationPass;
|
|
static uint8_t otaCalibrationLastStartSequence;
|
|
static uint8_t otaCalibrationLastStartOpcode;
|
|
static uint32_t otaCalibrationMoveActiveMask;
|
|
static uint32_t otaCalibrationMoveSavePendingMask;
|
|
static uint32_t otaCalibrationMoveFailedMask;
|
|
static uint8_t otaCalibrationMoveLastError;
|
|
static uint32_t otaCalibrationMoveLeaseTicks[MOTOR_PROFILE_COUNT];
|
|
static uint32_t otaCalibrationMoveSaveTicks;
|
|
static bool otaCalibrationDynamicActive;
|
|
static bool otaCalibrationDynamicMotorActive;
|
|
static ota_seat_dynamic_mode_t otaCalibrationDynamicMode;
|
|
static uint8_t otaCalibrationDynamicStep;
|
|
static uint8_t otaCalibrationDynamicAxis = 0x0Fu;
|
|
static uint16_t otaCalibrationDynamicBasePermille[MOTOR_PROFILE_COUNT];
|
|
static uint32_t otaCalibrationDynamicStartedTicks;
|
|
static uint32_t otaCalibrationDynamicDurationTicks;
|
|
static uint32_t otaCalibrationDynamicDwellTicks;
|
|
|
|
typedef struct
|
|
{
|
|
uint8_t axis;
|
|
int16_t offsetPermille;
|
|
} ota_dynamic_step_t;
|
|
|
|
static const ota_dynamic_step_t otaCradleSteps[] =
|
|
{
|
|
{ 2u, 25 }, { 3u, 30 }, { 2u, -25 }, { 3u, -30 }
|
|
};
|
|
|
|
static const ota_dynamic_step_t otaRockingHorseSteps[] =
|
|
{
|
|
{ 1u, 55 }, { 0u, 40 }, { 1u, -55 }, { 0u, -40 }
|
|
};
|
|
|
|
static uint32_t OtaCalibration_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 OtaCalibration_Trace(uint32_t event)
|
|
{
|
|
uint32_t savedIntrStatus;
|
|
uint32_t sequence;
|
|
uint32_t slot;
|
|
uint32_t axis = (uint32_t)otaSeatJobAxis;
|
|
uint32_t position = 0ul;
|
|
uint32_t range = 0ul;
|
|
|
|
savedIntrStatus = Cy_SysLib_EnterCriticalSection();
|
|
sequence = g_otaCalibrationTrace.nextSequence;
|
|
slot = sequence & (OTA_CALIBRATION_TRACE_CAPACITY - 1u);
|
|
if(axis < MOTOR_PROFILE_COUNT)
|
|
{
|
|
position = motorRuntime[axis].positionHallCount;
|
|
range = motorRuntime[axis].maxHallCount;
|
|
}
|
|
g_otaCalibrationTrace.events[slot].sequence = sequence;
|
|
g_otaCalibrationTrace.events[slot].event = event;
|
|
g_otaCalibrationTrace.events[slot].tick = hallTimebaseTicks;
|
|
g_otaCalibrationTrace.events[slot].jobState =
|
|
((uint32_t)otaSeatJobType) |
|
|
((uint32_t)otaSeatJobAxis << 8u) |
|
|
((uint32_t)otaSeatJobPhase << 16u) |
|
|
((uint32_t)otaSeatJobError << 24u);
|
|
g_otaCalibrationTrace.events[slot].position = position;
|
|
g_otaCalibrationTrace.events[slot].range = range;
|
|
g_otaCalibrationTrace.nextSequence = sequence + 1ul;
|
|
Cy_SysLib_ExitCriticalSection(savedIntrStatus);
|
|
}
|
|
|
|
static bool OtaCalibration_ReturnMatches(uint32_t outward,
|
|
uint32_t returned)
|
|
{
|
|
uint32_t minimum;
|
|
uint32_t maximum;
|
|
|
|
if(outward < OTA_CALIBRATION_MINIMUM_PULSES)
|
|
{
|
|
return false;
|
|
}
|
|
minimum = (outward *
|
|
(100ul - OTA_CALIBRATION_RETURN_TOLERANCE_PERCENT)) / 100ul;
|
|
maximum = (outward *
|
|
(100ul + OTA_CALIBRATION_RETURN_TOLERANCE_PERCENT)) / 100ul;
|
|
return (returned >= minimum) && (returned <= maximum);
|
|
}
|
|
|
|
static void OtaCalibration_RequestAbort(ota_calibration_error_t error)
|
|
{
|
|
uint32_t savedIntrStatus;
|
|
uint32_t axis = (uint32_t)otaSeatJobAxis;
|
|
bool firstAbort = (false == otaCalibrationAbortRequested);
|
|
|
|
otaCalibrationAbortRequested = true;
|
|
if((otaSeatJobError == OTA_CALIBRATION_ERROR_NONE) &&
|
|
(error != OTA_CALIBRATION_ERROR_NONE))
|
|
{
|
|
otaSeatJobError = (uint8_t)error;
|
|
}
|
|
if(false != firstAbort)
|
|
{
|
|
OtaCalibration_Trace(0x000000E0ul);
|
|
}
|
|
if((false == otaSeatJobActive) || (axis >= MOTOR_PROFILE_COUNT))
|
|
{
|
|
return;
|
|
}
|
|
savedIntrStatus = Cy_SysLib_EnterCriticalSection();
|
|
if((motorRuntime[axis].direction != MOTOR_DIRECTION_STOPPED) ||
|
|
(false != motorRuntime[axis].startPending))
|
|
{
|
|
Motor_RequestEventFromIsr(axis, MOTOR_EVENT_ABORTED);
|
|
}
|
|
Cy_SysLib_ExitCriticalSection(savedIntrStatus);
|
|
}
|
|
|
|
static bool OtaCalibration_MoveAxisSettled(uint32_t axis)
|
|
{
|
|
uint32_t savedIntrStatus;
|
|
bool settled;
|
|
|
|
if(axis >= MOTOR_PROFILE_COUNT)
|
|
{
|
|
return true;
|
|
}
|
|
savedIntrStatus = Cy_SysLib_EnterCriticalSection();
|
|
settled = (motorRuntime[axis].direction == MOTOR_DIRECTION_STOPPED) &&
|
|
(false == motorRuntime[axis].startPending) &&
|
|
(false == motorRuntime[axis].targetActive) &&
|
|
(false == motorRuntime[axis].coastTracking);
|
|
Cy_SysLib_ExitCriticalSection(savedIntrStatus);
|
|
return settled;
|
|
}
|
|
|
|
static bool OtaCalibration_AnyInteractiveMoveActive(void)
|
|
{
|
|
return (otaCalibrationMoveActiveMask != 0ul);
|
|
}
|
|
|
|
static uint8_t OtaCalibration_InteractiveJobAxis(uint32_t activeMask,
|
|
uint8_t fallbackAxis)
|
|
{
|
|
uint32_t axis;
|
|
uint8_t first = OTA_SEAT_JOB_AXIS_MULTIPLE;
|
|
|
|
for(axis = 0ul; axis < MOTOR_PROFILE_COUNT; axis++)
|
|
{
|
|
if((activeMask & (1ul << axis)) == 0ul)
|
|
{
|
|
continue;
|
|
}
|
|
if(first != OTA_SEAT_JOB_AXIS_MULTIPLE)
|
|
{
|
|
return OTA_SEAT_JOB_AXIS_MULTIPLE;
|
|
}
|
|
first = (uint8_t)axis;
|
|
}
|
|
return (first == OTA_SEAT_JOB_AXIS_MULTIPLE) ? fallbackAxis : first;
|
|
}
|
|
|
|
static bool OtaCalibration_AllInteractiveAxesSettled(void)
|
|
{
|
|
uint32_t axis;
|
|
|
|
for(axis = 0ul; axis < MOTOR_PROFILE_COUNT; axis++)
|
|
{
|
|
if((otaCalibrationMoveSavePendingMask & (1ul << axis)) != 0ul)
|
|
{
|
|
if(false == OtaCalibration_MoveAxisSettled(axis))
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
static void OtaCalibration_PublishInteractiveJob(uint8_t fallbackAxis)
|
|
{
|
|
bool active = OtaCalibration_AnyInteractiveMoveActive();
|
|
|
|
otaSeatJobActive = active;
|
|
otaSeatJobComplete = (false == active);
|
|
otaSeatJobSuccess = (false == active) &&
|
|
(otaCalibrationMoveFailedMask == 0ul);
|
|
otaSeatJobType = OTA_SEAT_JOB_MOVE;
|
|
otaSeatJobPhase = (false != active) ? OTA_SEAT_PHASE_MOVING :
|
|
((otaCalibrationMoveFailedMask == 0ul) ?
|
|
OTA_SEAT_PHASE_STOPPED : OTA_SEAT_PHASE_FAILED);
|
|
otaSeatJobAxis = OtaCalibration_InteractiveJobAxis(
|
|
otaCalibrationMoveActiveMask, fallbackAxis);
|
|
otaSeatJobError = (otaCalibrationMoveFailedMask == 0ul) ?
|
|
OTA_CALIBRATION_ERROR_NONE :
|
|
otaCalibrationMoveLastError;
|
|
}
|
|
|
|
static uint8_t OtaCalibration_DefaultInteractiveDuty(uint32_t axis)
|
|
{
|
|
return (axis == (uint32_t)MOTOR_MODE_3_LEGREST) ?
|
|
(uint8_t)MOTOR_LEGREST_DEFAULT_DUTY_PERCENT :
|
|
(uint8_t)MOTOR_PWM_DUTY_PERCENT;
|
|
}
|
|
|
|
static void OtaCalibration_FinalizeInteractiveAxis(
|
|
uint32_t axis,
|
|
ota_calibration_error_t error,
|
|
bool success,
|
|
bool stopMotor)
|
|
{
|
|
uint32_t bit;
|
|
|
|
if(axis >= MOTOR_PROFILE_COUNT)
|
|
{
|
|
return;
|
|
}
|
|
bit = 1ul << axis;
|
|
if((otaCalibrationMoveActiveMask & bit) == 0ul)
|
|
{
|
|
return;
|
|
}
|
|
if(false != stopMotor)
|
|
{
|
|
(void)Motor_StopMode((motor_mode_t)axis);
|
|
}
|
|
/* Explicit duty belongs only to the completed interactive move. Restore
|
|
* the production default before a later 0x64 move or dynamic mode starts. */
|
|
(void)Motor_SetDutyPercent(
|
|
(motor_mode_t)axis, OtaCalibration_DefaultInteractiveDuty(axis));
|
|
otaCalibrationMoveActiveMask &= ~bit;
|
|
otaCalibrationMoveSavePendingMask |= bit;
|
|
otaCalibrationMoveSaveTicks = hallTimebaseTicks;
|
|
if(false == success)
|
|
{
|
|
otaCalibrationMoveFailedMask |= bit;
|
|
otaCalibrationMoveLastError = (uint8_t)error;
|
|
}
|
|
OtaCalibration_PublishInteractiveJob((uint8_t)axis);
|
|
}
|
|
|
|
static void OtaCalibration_FinalizeAllInteractiveMoves(
|
|
ota_calibration_error_t error,
|
|
bool success,
|
|
bool stopMotor)
|
|
{
|
|
uint32_t axis;
|
|
|
|
for(axis = 0ul; axis < MOTOR_PROFILE_COUNT; axis++)
|
|
{
|
|
if((otaCalibrationMoveActiveMask & (1ul << axis)) != 0ul)
|
|
{
|
|
OtaCalibration_FinalizeInteractiveAxis(
|
|
axis, error, success, stopMotor);
|
|
}
|
|
}
|
|
}
|
|
|
|
static const ota_dynamic_step_t *OtaCalibration_DynamicSteps(
|
|
uint32_t *count)
|
|
{
|
|
if(otaCalibrationDynamicMode == OTA_SEAT_DYNAMIC_CRADLE)
|
|
{
|
|
*count = (uint32_t)(sizeof(otaCradleSteps) / sizeof(otaCradleSteps[0]));
|
|
return otaCradleSteps;
|
|
}
|
|
*count = (uint32_t)(sizeof(otaRockingHorseSteps) /
|
|
sizeof(otaRockingHorseSteps[0]));
|
|
return otaRockingHorseSteps;
|
|
}
|
|
|
|
static void OtaCalibration_StopDynamic(
|
|
ota_calibration_error_t error,
|
|
bool success)
|
|
{
|
|
uint32_t axis = (uint32_t)otaCalibrationDynamicAxis;
|
|
|
|
if((false != otaCalibrationDynamicMotorActive) &&
|
|
(axis < MOTOR_PROFILE_COUNT))
|
|
{
|
|
(void)Motor_StopMode((motor_mode_t)axis);
|
|
}
|
|
otaCalibrationDynamicActive = false;
|
|
otaCalibrationDynamicMotorActive = false;
|
|
if(axis < MOTOR_PROFILE_COUNT)
|
|
{
|
|
otaCalibrationMoveSavePendingMask |= 1ul << axis;
|
|
}
|
|
otaCalibrationMoveSaveTicks = hallTimebaseTicks;
|
|
otaSeatJobActive = false;
|
|
otaSeatJobComplete = true;
|
|
otaSeatJobSuccess = success;
|
|
otaSeatJobType = OTA_SEAT_JOB_DYNAMIC;
|
|
otaSeatJobPhase = OTA_SEAT_PHASE_STOPPED;
|
|
otaSeatJobAxis = (uint8_t)axis;
|
|
otaSeatJobError = (uint8_t)error;
|
|
otaSeatJobProgress = 100u;
|
|
}
|
|
|
|
static bool OtaCalibration_StartDynamicStep(void)
|
|
{
|
|
const ota_dynamic_step_t *steps;
|
|
const ota_dynamic_step_t *step;
|
|
uint32_t count;
|
|
uint32_t axis;
|
|
int32_t targetPermille;
|
|
uint32_t targetValueX100;
|
|
|
|
steps = OtaCalibration_DynamicSteps(&count);
|
|
if((0ul == count) || (otaCalibrationDynamicStep >= count))
|
|
{
|
|
otaCalibrationDynamicStep = 0u;
|
|
}
|
|
step = &steps[otaCalibrationDynamicStep];
|
|
axis = (uint32_t)step->axis;
|
|
if(axis >= MOTOR_PROFILE_COUNT)
|
|
{
|
|
return false;
|
|
}
|
|
targetPermille = (int32_t)otaCalibrationDynamicBasePermille[axis] +
|
|
(int32_t)step->offsetPermille;
|
|
if(targetPermille < OTA_DYNAMIC_MIN_TARGET_PERMILLE)
|
|
{
|
|
targetPermille = OTA_DYNAMIC_MIN_TARGET_PERMILLE;
|
|
}
|
|
if(targetPermille > OTA_DYNAMIC_MAX_TARGET_PERMILLE)
|
|
{
|
|
targetPermille = OTA_DYNAMIC_MAX_TARGET_PERMILLE;
|
|
}
|
|
targetValueX100 = (uint32_t)
|
|
(((((uint64_t)(uint32_t)targetPermille) *
|
|
motorProfiles[axis].maxPhysicalValueX100) + 500ul) / 1000ul);
|
|
if(MOTOR_API_SUCCESS != Motor_StartMoveToValue(
|
|
(motor_mode_t)axis, targetValueX100))
|
|
{
|
|
return false;
|
|
}
|
|
otaCalibrationDynamicAxis = (uint8_t)axis;
|
|
otaCalibrationDynamicMotorActive = true;
|
|
otaSeatPersistedMask &= ~(1ul << axis);
|
|
otaSeatJobAxis = (uint8_t)axis;
|
|
otaSeatJobPhase = OTA_SEAT_PHASE_DYNAMIC;
|
|
return true;
|
|
}
|
|
|
|
static void OtaCalibration_ServiceDynamicMode(void)
|
|
{
|
|
uint32_t axis;
|
|
uint32_t savedIntrStatus;
|
|
uint32_t elapsed;
|
|
motor_event_t event = MOTOR_EVENT_NONE;
|
|
bool settled;
|
|
|
|
if(false == otaCalibrationDynamicActive)
|
|
{
|
|
return;
|
|
}
|
|
if(false != driverFaultLatched)
|
|
{
|
|
OtaCalibration_StopDynamic(OTA_CALIBRATION_ERROR_DRIVER, false);
|
|
return;
|
|
}
|
|
if((false != OtaCan_IsBusOff()) ||
|
|
(false != OtaCan_BusOffOccurred()) ||
|
|
(false != OtaCan_RxOverflowed()))
|
|
{
|
|
OtaCalibration_StopDynamic(OTA_CALIBRATION_ERROR_CAN, false);
|
|
return;
|
|
}
|
|
elapsed = (uint32_t)(hallTimebaseTicks -
|
|
otaCalibrationDynamicStartedTicks);
|
|
if(elapsed >= otaCalibrationDynamicDurationTicks)
|
|
{
|
|
OtaCalibration_StopDynamic(OTA_CALIBRATION_ERROR_NONE, true);
|
|
return;
|
|
}
|
|
otaSeatJobProgress = (uint8_t)
|
|
((elapsed * 100ul) / otaCalibrationDynamicDurationTicks);
|
|
|
|
if(false == otaCalibrationDynamicMotorActive)
|
|
{
|
|
if((uint32_t)(hallTimebaseTicks - otaCalibrationDynamicDwellTicks) <
|
|
MOTOR_MS_TO_TICKS(OTA_DYNAMIC_DWELL_MS))
|
|
{
|
|
return;
|
|
}
|
|
if(false == OtaCalibration_StartDynamicStep())
|
|
{
|
|
OtaCalibration_StopDynamic(OTA_CALIBRATION_ERROR_MOTOR, false);
|
|
}
|
|
return;
|
|
}
|
|
|
|
axis = (uint32_t)otaCalibrationDynamicAxis;
|
|
if(axis >= MOTOR_PROFILE_COUNT)
|
|
{
|
|
OtaCalibration_StopDynamic(OTA_CALIBRATION_ERROR_MOTOR, false);
|
|
return;
|
|
}
|
|
savedIntrStatus = Cy_SysLib_EnterCriticalSection();
|
|
settled = (motorRuntime[axis].direction == MOTOR_DIRECTION_STOPPED) &&
|
|
(false == motorRuntime[axis].startPending) &&
|
|
(false == motorRuntime[axis].targetActive) &&
|
|
(false == motorRuntime[axis].coastTracking);
|
|
event = motorRuntime[axis].lastEvent;
|
|
Cy_SysLib_ExitCriticalSection(savedIntrStatus);
|
|
if(false != settled)
|
|
{
|
|
uint32_t count;
|
|
(void)OtaCalibration_DynamicSteps(&count);
|
|
if((event != MOTOR_EVENT_TARGET_REACHED) &&
|
|
(event != MOTOR_EVENT_NEAR_ENDPOINT) &&
|
|
(event != MOTOR_EVENT_FAR_ENDPOINT))
|
|
{
|
|
OtaCalibration_StopDynamic(OTA_CALIBRATION_ERROR_MOTOR, false);
|
|
return;
|
|
}
|
|
otaCalibrationDynamicMotorActive = false;
|
|
otaCalibrationDynamicStep = (uint8_t)
|
|
(((uint32_t)otaCalibrationDynamicStep + 1ul) % count);
|
|
otaCalibrationDynamicDwellTicks = hallTimebaseTicks;
|
|
}
|
|
}
|
|
|
|
static void OtaCalibration_ServiceInteractiveMove(void)
|
|
{
|
|
uint32_t axis;
|
|
|
|
if(false != OtaCalibration_AnyInteractiveMoveActive())
|
|
{
|
|
if(false != driverFaultLatched)
|
|
{
|
|
OtaCalibration_FinalizeAllInteractiveMoves(
|
|
OTA_CALIBRATION_ERROR_DRIVER, false, true);
|
|
}
|
|
else if((false != OtaCan_IsBusOff()) ||
|
|
(false != OtaCan_BusOffOccurred()) ||
|
|
(false != OtaCan_RxOverflowed()))
|
|
{
|
|
OtaCalibration_FinalizeAllInteractiveMoves(
|
|
OTA_CALIBRATION_ERROR_CAN, false, true);
|
|
}
|
|
else
|
|
{
|
|
for(axis = 0ul; axis < MOTOR_PROFILE_COUNT; axis++)
|
|
{
|
|
uint32_t savedIntrStatus;
|
|
motor_event_t event = MOTOR_EVENT_NONE;
|
|
bool stopped;
|
|
bool success;
|
|
|
|
if((otaCalibrationMoveActiveMask & (1ul << axis)) == 0ul)
|
|
{
|
|
continue;
|
|
}
|
|
if((uint32_t)(hallTimebaseTicks -
|
|
otaCalibrationMoveLeaseTicks[axis]) >=
|
|
MOTOR_MS_TO_TICKS(OTA_INTERACTIVE_MOVE_LEASE_MS))
|
|
{
|
|
OtaCalibration_FinalizeInteractiveAxis(
|
|
axis, OTA_CALIBRATION_ERROR_LEASE, false, true);
|
|
continue;
|
|
}
|
|
|
|
savedIntrStatus = Cy_SysLib_EnterCriticalSection();
|
|
stopped = (motorRuntime[axis].direction ==
|
|
MOTOR_DIRECTION_STOPPED) &&
|
|
(false == motorRuntime[axis].startPending) &&
|
|
(false == motorRuntime[axis].targetActive) &&
|
|
(false == motorRuntime[axis].coastTracking);
|
|
event = motorRuntime[axis].lastEvent;
|
|
Cy_SysLib_ExitCriticalSection(savedIntrStatus);
|
|
if(false == stopped)
|
|
{
|
|
continue;
|
|
}
|
|
success = (event == MOTOR_EVENT_TARGET_REACHED) ||
|
|
(event == MOTOR_EVENT_NEAR_ENDPOINT) ||
|
|
(event == MOTOR_EVENT_FAR_ENDPOINT);
|
|
OtaCalibration_FinalizeInteractiveAxis(
|
|
axis,
|
|
(false != success) ? OTA_CALIBRATION_ERROR_NONE :
|
|
OTA_CALIBRATION_ERROR_MOTOR,
|
|
success, false);
|
|
}
|
|
}
|
|
}
|
|
|
|
if((otaCalibrationMoveSavePendingMask != 0ul) &&
|
|
(false == OtaCalibration_AnyInteractiveMoveActive()) &&
|
|
(false != OtaCalibration_AllInteractiveAxesSettled()) &&
|
|
((uint32_t)(hallTimebaseTicks - otaCalibrationMoveSaveTicks) >=
|
|
MOTOR_MS_TO_TICKS(OTA_INTERACTIVE_MOVE_SAVE_DELAY_MS)))
|
|
{
|
|
if(false == OtaSeat_SavePersistentState())
|
|
{
|
|
otaSeatJobSuccess = false;
|
|
otaSeatJobPhase = OTA_SEAT_PHASE_FAILED;
|
|
otaSeatJobError = OTA_CALIBRATION_ERROR_STORAGE;
|
|
}
|
|
otaCalibrationMoveSavePendingMask = 0ul;
|
|
}
|
|
}
|
|
|
|
static void OtaCalibration_UpdateProgress(void)
|
|
{
|
|
uint32_t axis = (uint32_t)otaSeatJobAxis;
|
|
uint32_t position;
|
|
uint32_t range;
|
|
uint32_t savedIntrStatus;
|
|
uint32_t progress;
|
|
|
|
if((false == otaSeatJobActive) || (axis >= MOTOR_PROFILE_COUNT))
|
|
{
|
|
return;
|
|
}
|
|
|
|
if(otaSeatJobType == OTA_SEAT_JOB_MOVE)
|
|
{
|
|
savedIntrStatus = Cy_SysLib_EnterCriticalSection();
|
|
position = motorRuntime[axis].positionHallCount;
|
|
range = motorRuntime[axis].maxHallCount;
|
|
Cy_SysLib_ExitCriticalSection(savedIntrStatus);
|
|
otaSeatJobPhase = OTA_SEAT_PHASE_MOVING;
|
|
otaSeatJobProgress = (uint8_t)((range > 0ul) ?
|
|
(((position > range ? range : position) * 100ul) / range) : 0ul);
|
|
return;
|
|
}
|
|
if(otaSeatJobType == OTA_SEAT_JOB_DYNAMIC)
|
|
{
|
|
/* The board-owned pattern service publishes elapsed progress. */
|
|
return;
|
|
}
|
|
progress = axis * 25ul;
|
|
savedIntrStatus = Cy_SysLib_EnterCriticalSection();
|
|
position = motorRuntime[axis].positionHallCount;
|
|
range = motorRuntime[axis].maxHallCount;
|
|
if(otaCalibrationPass == 0u)
|
|
{
|
|
if(motorRuntime[axis].direction == MOTOR_DIRECTION_TOWARD_FAR)
|
|
{
|
|
otaSeatJobPhase = OTA_SEAT_PHASE_FIND_FAR;
|
|
}
|
|
else if(motorRuntime[axis].direction == MOTOR_DIRECTION_TOWARD_NEAR)
|
|
{
|
|
otaSeatJobPhase = OTA_SEAT_PHASE_FIND_NEAR;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
otaSeatJobPhase = OTA_SEAT_PHASE_RETURN_NEAR;
|
|
}
|
|
Cy_SysLib_ExitCriticalSection(savedIntrStatus);
|
|
|
|
if(otaSeatJobType == OTA_SEAT_JOB_HOME)
|
|
{
|
|
if((range > 0ul) && (position <= range))
|
|
{
|
|
progress += ((range - position) * 24ul) / range;
|
|
}
|
|
}
|
|
else if(otaSeatJobPhase == OTA_SEAT_PHASE_FIND_FAR)
|
|
{
|
|
progress += 8ul;
|
|
if((range > 0ul) && (position <= range))
|
|
{
|
|
progress += (position * 8ul) / range;
|
|
}
|
|
}
|
|
else if(otaSeatJobPhase == OTA_SEAT_PHASE_RETURN_NEAR)
|
|
{
|
|
progress += 16ul;
|
|
if((range > 0ul) && (position <= range))
|
|
{
|
|
progress += ((range - position) * 8ul) / range;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
progress += 4ul;
|
|
}
|
|
if(progress > 99ul) progress = 99ul;
|
|
otaSeatJobProgress = (uint8_t)progress;
|
|
}
|
|
|
|
static void OtaCalibration_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 OtaCalibration_TestState(void)
|
|
{
|
|
uint32_t value = OTA_TEST_STATE_PREPARED;
|
|
if(false != otaSeatJobActive) value |= OTA_TEST_STATE_ACTIVE;
|
|
if(false != otaSeatJobComplete) value |= OTA_TEST_STATE_COMPLETE;
|
|
if(false != driverFaultLatched) value |= OTA_TEST_STATE_DRIVER_FAULT;
|
|
return value;
|
|
}
|
|
|
|
static void OtaCalibration_HandleInteractiveMove(
|
|
const ota_can_control_frame_t *frame)
|
|
{
|
|
uint32_t axis = (uint32_t)frame->data[3];
|
|
uint32_t argument = OtaCalibration_ReadLe32(&frame->data[4]);
|
|
uint32_t targetPermille = argument;
|
|
uint32_t targetValueX100;
|
|
uint32_t savedIntrStatus;
|
|
bool positionValid;
|
|
uint32_t range;
|
|
motor_safety_options_t safety = { false, false };
|
|
uint32_t bit;
|
|
uint8_t dutyPercent;
|
|
bool explicitDuty =
|
|
(frame->data[2] == (uint8_t)OTA_OPCODE_SEAT_MOVE_WITH_DUTY);
|
|
|
|
if(axis >= MOTOR_PROFILE_COUNT)
|
|
{
|
|
OtaCalibration_Respond(frame, OTA_STATUS_BAD_ARGUMENT,
|
|
argument);
|
|
return;
|
|
}
|
|
dutyPercent = OtaCalibration_DefaultInteractiveDuty(axis);
|
|
if(false != explicitDuty)
|
|
{
|
|
targetPermille = argument & OTA_INTERACTIVE_MOVE_TARGET_MASK;
|
|
dutyPercent = (uint8_t)
|
|
((argument >> OTA_INTERACTIVE_MOVE_DUTY_SHIFT) & 0xFFul);
|
|
}
|
|
if((targetPermille > OTA_INTERACTIVE_MOVE_TARGET_MAX) ||
|
|
((false != explicitDuty) &&
|
|
(((argument & OTA_INTERACTIVE_MOVE_RESERVED_MASK) != 0ul) ||
|
|
(dutyPercent < MOTOR_FIXED_DUTY_MIN_PERCENT) ||
|
|
(dutyPercent > MOTOR_FIXED_DUTY_MAX_PERCENT))))
|
|
{
|
|
OtaCalibration_Respond(frame, OTA_STATUS_BAD_ARGUMENT, argument);
|
|
return;
|
|
}
|
|
if(false != OtaCan_IsBusOff())
|
|
{
|
|
if(false != OtaCalibration_AnyInteractiveMoveActive())
|
|
{
|
|
OtaCalibration_FinalizeAllInteractiveMoves(
|
|
OTA_CALIBRATION_ERROR_CAN, false, true);
|
|
}
|
|
OtaCalibration_Respond(frame, OTA_STATUS_BUS_OFF,
|
|
OtaSeat_JobStateValue());
|
|
return;
|
|
}
|
|
if(false != OtaCan_RxOverflowed())
|
|
{
|
|
if(false != OtaCalibration_AnyInteractiveMoveActive())
|
|
{
|
|
OtaCalibration_FinalizeAllInteractiveMoves(
|
|
OTA_CALIBRATION_ERROR_CAN, false, true);
|
|
OtaCalibration_Respond(frame, OTA_STATUS_RX_OVERFLOW,
|
|
OtaSeat_JobStateValue());
|
|
return;
|
|
}
|
|
OtaCan_ClearLatchedFaults();
|
|
}
|
|
if(((false != otaSeatJobActive) &&
|
|
(otaSeatJobType != OTA_SEAT_JOB_MOVE)) ||
|
|
(otaCalibrationPendingJob != OTA_SEAT_JOB_NONE) ||
|
|
(false != otaCalibrationPendingBootloader))
|
|
{
|
|
OtaCalibration_Respond(frame, OTA_STATUS_BAD_STATE,
|
|
OtaSeat_JobStateValue());
|
|
return;
|
|
}
|
|
if(false != driverFaultLatched)
|
|
{
|
|
OtaCalibration_Respond(frame, OTA_STATUS_BAD_STATE,
|
|
OtaSeat_JobStateValue());
|
|
return;
|
|
}
|
|
|
|
savedIntrStatus = Cy_SysLib_EnterCriticalSection();
|
|
positionValid = motorRuntime[axis].positionValid;
|
|
range = motorRuntime[axis].maxHallCount;
|
|
Cy_SysLib_ExitCriticalSection(savedIntrStatus);
|
|
if((false == positionValid) || (0ul == range))
|
|
{
|
|
OtaCalibration_Respond(frame, OTA_STATUS_BAD_STATE,
|
|
OtaSeat_AxisPositionRangeValue(axis));
|
|
return;
|
|
}
|
|
|
|
/* Native 0x64/0x67 owns only the addressed axis. */
|
|
SeatDemoCompat_ReleaseAxisForNative(axis);
|
|
|
|
bit = 1ul << axis;
|
|
if(false == OtaCalibration_AnyInteractiveMoveActive())
|
|
{
|
|
OtaCan_ClearLatchedFaults();
|
|
Cy_GPIO_Clr(DRV_DRVOFF_PORT, DRV_DRVOFF_PIN);
|
|
Cy_SysTick_DelayInUs(100ul);
|
|
otaCalibrationMoveFailedMask = 0ul;
|
|
otaCalibrationMoveLastError = OTA_CALIBRATION_ERROR_NONE;
|
|
}
|
|
if(((otaCalibrationMoveActiveMask & bit) == 0ul) &&
|
|
((MOTOR_API_SUCCESS != Motor_SetSafetyOptions(
|
|
(motor_mode_t)axis, &safety)) ||
|
|
(MOTOR_API_SUCCESS != Motor_DisableSpeedControl(
|
|
(motor_mode_t)axis))))
|
|
{
|
|
OtaCalibration_Respond(frame, OTA_STATUS_BAD_STATE,
|
|
OtaSeat_JobStateValue());
|
|
return;
|
|
}
|
|
if(MOTOR_API_SUCCESS != Motor_SetDutyPercent(
|
|
(motor_mode_t)axis, dutyPercent))
|
|
{
|
|
OtaCalibration_Respond(frame, OTA_STATUS_BAD_STATE,
|
|
OtaSeat_JobStateValue());
|
|
return;
|
|
}
|
|
|
|
targetValueX100 = (uint32_t)
|
|
(((((uint64_t)targetPermille) *
|
|
motorProfiles[axis].maxPhysicalValueX100) + 500ul) / 1000ul);
|
|
if(MOTOR_API_SUCCESS != Motor_StartMoveToValue(
|
|
(motor_mode_t)axis, targetValueX100))
|
|
{
|
|
if((otaCalibrationMoveActiveMask & bit) != 0ul)
|
|
{
|
|
OtaCalibration_FinalizeInteractiveAxis(
|
|
axis,
|
|
OTA_CALIBRATION_ERROR_MOTOR, false, true);
|
|
}
|
|
OtaCalibration_Respond(frame, OTA_STATUS_BAD_STATE,
|
|
OtaSeat_JobStateValue());
|
|
return;
|
|
}
|
|
|
|
otaCalibrationMoveActiveMask |= bit;
|
|
otaCalibrationMoveFailedMask &= ~bit;
|
|
otaCalibrationMoveLeaseTicks[axis] = hallTimebaseTicks;
|
|
otaSeatPersistedMask &= ~bit;
|
|
otaSeatJobProgress = (uint8_t)(targetPermille / 10ul);
|
|
OtaCalibration_PublishInteractiveJob((uint8_t)axis);
|
|
OtaCalibration_Respond(frame, OTA_STATUS_OK,
|
|
OtaSeat_JobStateValue());
|
|
}
|
|
|
|
static void OtaCalibration_HandleInteractiveHeartbeat(
|
|
const ota_can_control_frame_t *frame)
|
|
{
|
|
uint32_t axis = (uint32_t)frame->data[3];
|
|
uint32_t renewMask;
|
|
uint32_t renewAxis;
|
|
|
|
if(false == OtaCalibration_AnyInteractiveMoveActive())
|
|
{
|
|
OtaCalibration_Respond(frame, OTA_STATUS_BAD_STATE,
|
|
OtaSeat_JobStateValue());
|
|
return;
|
|
}
|
|
if(false != OtaCan_IsBusOff())
|
|
{
|
|
OtaCalibration_FinalizeAllInteractiveMoves(
|
|
OTA_CALIBRATION_ERROR_CAN, false, true);
|
|
OtaCalibration_Respond(frame, OTA_STATUS_BUS_OFF,
|
|
OtaSeat_JobStateValue());
|
|
return;
|
|
}
|
|
if(false != OtaCan_RxOverflowed())
|
|
{
|
|
OtaCalibration_FinalizeAllInteractiveMoves(
|
|
OTA_CALIBRATION_ERROR_CAN, false, true);
|
|
OtaCalibration_Respond(frame, OTA_STATUS_RX_OVERFLOW,
|
|
OtaSeat_JobStateValue());
|
|
return;
|
|
}
|
|
if(axis == (uint32_t)OTA_SEAT_AXIS_ALL)
|
|
{
|
|
renewMask = otaCalibrationMoveActiveMask;
|
|
}
|
|
else if((axis < MOTOR_PROFILE_COUNT) &&
|
|
((otaCalibrationMoveActiveMask & (1ul << axis)) != 0ul))
|
|
{
|
|
renewMask = 1ul << axis;
|
|
}
|
|
else
|
|
{
|
|
OtaCalibration_Respond(frame, OTA_STATUS_BAD_STATE,
|
|
OtaSeat_JobStateValue());
|
|
return;
|
|
}
|
|
for(renewAxis = 0ul; renewAxis < MOTOR_PROFILE_COUNT; renewAxis++)
|
|
{
|
|
if((renewMask & (1ul << renewAxis)) != 0ul)
|
|
{
|
|
otaCalibrationMoveLeaseTicks[renewAxis] = hallTimebaseTicks;
|
|
}
|
|
}
|
|
OtaCalibration_Respond(frame, OTA_STATUS_OK,
|
|
OtaSeat_JobStateValue());
|
|
}
|
|
|
|
static void OtaCalibration_HandleDynamicStart(
|
|
const ota_can_control_frame_t *frame)
|
|
{
|
|
uint32_t mode = (uint32_t)frame->data[3];
|
|
uint32_t durationSeconds = OtaCalibration_ReadLe32(&frame->data[4]);
|
|
uint32_t axis;
|
|
uint32_t savedIntrStatus;
|
|
motor_safety_options_t safety = { false, false };
|
|
|
|
if((mode != (uint32_t)OTA_SEAT_DYNAMIC_CRADLE) &&
|
|
(mode != (uint32_t)OTA_SEAT_DYNAMIC_ROCKING_HORSE))
|
|
{
|
|
OtaCalibration_Respond(frame, OTA_STATUS_BAD_ARGUMENT, mode);
|
|
return;
|
|
}
|
|
if(0ul == durationSeconds)
|
|
{
|
|
durationSeconds = OTA_DYNAMIC_DEFAULT_DURATION_SECONDS;
|
|
}
|
|
if((durationSeconds < OTA_DYNAMIC_MIN_DURATION_SECONDS) ||
|
|
(durationSeconds > OTA_DYNAMIC_MAX_DURATION_SECONDS))
|
|
{
|
|
OtaCalibration_Respond(frame, OTA_STATUS_BAD_ARGUMENT,
|
|
durationSeconds);
|
|
return;
|
|
}
|
|
if((false != OtaCalibration_AnyInteractiveMoveActive()) ||
|
|
(false != otaCalibrationDynamicActive) ||
|
|
(false != otaSeatJobActive) ||
|
|
(otaCalibrationPendingJob != OTA_SEAT_JOB_NONE) ||
|
|
(false != otaCalibrationPendingBootloader) ||
|
|
(false != driverFaultLatched))
|
|
{
|
|
OtaCalibration_Respond(frame, OTA_STATUS_BAD_STATE,
|
|
OtaSeat_JobStateValue());
|
|
return;
|
|
}
|
|
if(false != OtaCan_IsBusOff())
|
|
{
|
|
OtaCalibration_Respond(frame, OTA_STATUS_BUS_OFF,
|
|
OtaSeat_JobStateValue());
|
|
return;
|
|
}
|
|
|
|
for(axis = 0ul; axis < MOTOR_PROFILE_COUNT; axis++)
|
|
{
|
|
bool valid;
|
|
uint32_t range;
|
|
uint32_t position;
|
|
|
|
savedIntrStatus = Cy_SysLib_EnterCriticalSection();
|
|
valid = motorRuntime[axis].positionValid;
|
|
range = motorRuntime[axis].maxHallCount;
|
|
position = motorRuntime[axis].positionHallCount;
|
|
Cy_SysLib_ExitCriticalSection(savedIntrStatus);
|
|
if((false == valid) || (0ul == range))
|
|
{
|
|
OtaCalibration_Respond(frame, OTA_STATUS_BAD_STATE,
|
|
OtaSeat_AxisPositionRangeValue(axis));
|
|
return;
|
|
}
|
|
otaCalibrationDynamicBasePermille[axis] = (uint16_t)
|
|
(((position > range ? range : position) * 1000ul) / range);
|
|
if((MOTOR_API_SUCCESS != Motor_SetSafetyOptions(
|
|
(motor_mode_t)axis, &safety)) ||
|
|
(MOTOR_API_SUCCESS != Motor_DisableSpeedControl(
|
|
(motor_mode_t)axis)))
|
|
{
|
|
OtaCalibration_Respond(frame, OTA_STATUS_BAD_STATE, axis);
|
|
return;
|
|
}
|
|
}
|
|
|
|
SeatDemoCompat_StopAllFromNative();
|
|
OtaCan_ClearLatchedFaults();
|
|
Cy_GPIO_Clr(DRV_DRVOFF_PORT, DRV_DRVOFF_PIN);
|
|
Cy_SysTick_DelayInUs(100ul);
|
|
otaCalibrationDynamicMode = (ota_seat_dynamic_mode_t)mode;
|
|
otaCalibrationDynamicStep = 0u;
|
|
otaCalibrationDynamicAxis = 0x0Fu;
|
|
otaCalibrationDynamicMotorActive = false;
|
|
otaCalibrationDynamicStartedTicks = hallTimebaseTicks;
|
|
otaCalibrationDynamicDurationTicks =
|
|
MOTOR_MS_TO_TICKS(durationSeconds * 1000ul);
|
|
otaCalibrationDynamicDwellTicks = hallTimebaseTicks -
|
|
MOTOR_MS_TO_TICKS(OTA_DYNAMIC_DWELL_MS);
|
|
otaCalibrationDynamicActive = true;
|
|
otaCalibrationMoveSavePendingMask = 0ul;
|
|
otaSeatJobActive = true;
|
|
otaSeatJobComplete = false;
|
|
otaSeatJobSuccess = false;
|
|
otaSeatJobType = OTA_SEAT_JOB_DYNAMIC;
|
|
otaSeatJobPhase = OTA_SEAT_PHASE_DYNAMIC;
|
|
otaSeatJobAxis = 0x0Fu;
|
|
otaSeatJobError = OTA_CALIBRATION_ERROR_NONE;
|
|
otaSeatJobProgress = 0u;
|
|
OtaCalibration_Respond(frame, OTA_STATUS_OK,
|
|
OtaSeat_JobStateValue());
|
|
}
|
|
|
|
static void OtaCalibration_HandleControl(
|
|
const ota_can_control_frame_t *frame)
|
|
{
|
|
uint8_t opcode = frame->data[2];
|
|
|
|
if(frame->data[0] != OTA_CONTROL_MAGIC)
|
|
{
|
|
OtaCalibration_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:
|
|
OtaCalibration_HandleInteractiveMove(frame);
|
|
break;
|
|
case OTA_OPCODE_SEAT_HEARTBEAT:
|
|
OtaCalibration_HandleInteractiveHeartbeat(frame);
|
|
break;
|
|
case OTA_OPCODE_SEAT_DYNAMIC_START:
|
|
OtaCalibration_HandleDynamicStart(frame);
|
|
break;
|
|
case OTA_OPCODE_SEAT_CALIBRATE:
|
|
case OTA_OPCODE_SEAT_HOME:
|
|
if((frame->data[1] == otaCalibrationLastStartSequence) &&
|
|
(opcode == otaCalibrationLastStartOpcode) &&
|
|
((false != otaSeatJobActive) ||
|
|
(otaCalibrationPendingJob != OTA_SEAT_JOB_NONE)))
|
|
{
|
|
OtaCalibration_Respond(frame, OTA_STATUS_OK,
|
|
OtaSeat_JobStateValue());
|
|
}
|
|
else if((false != otaSeatJobActive) ||
|
|
(otaCalibrationPendingJob != OTA_SEAT_JOB_NONE))
|
|
{
|
|
OtaCalibration_Respond(frame, OTA_STATUS_BAD_STATE,
|
|
OtaSeat_JobStateValue());
|
|
}
|
|
else
|
|
{
|
|
SeatDemoCompat_StopAllFromNative();
|
|
otaCalibrationMoveSavePendingMask = 0ul;
|
|
otaCalibrationLastStartSequence = frame->data[1];
|
|
otaCalibrationLastStartOpcode = opcode;
|
|
otaCalibrationPendingJob =
|
|
(opcode == OTA_OPCODE_SEAT_CALIBRATE) ?
|
|
OTA_SEAT_JOB_CALIBRATE : OTA_SEAT_JOB_HOME;
|
|
/* Publish the accepted task before its blocking worker begins so
|
|
* status queries immediately observe the running operation. */
|
|
otaCalibrationAbortRequested = false;
|
|
otaSeatJobActive = true;
|
|
otaSeatJobComplete = false;
|
|
otaSeatJobSuccess = false;
|
|
otaSeatJobType = otaCalibrationPendingJob;
|
|
otaSeatJobPhase = OTA_SEAT_PHASE_FIND_NEAR;
|
|
otaSeatJobAxis = 0u;
|
|
otaSeatJobError = OTA_CALIBRATION_ERROR_NONE;
|
|
otaSeatJobProgress = 0u;
|
|
OtaCalibration_Trace(0x00000001ul);
|
|
OtaCalibration_Respond(frame, OTA_STATUS_OK,
|
|
OtaSeat_JobStateValue());
|
|
}
|
|
break;
|
|
case OTA_OPCODE_SEAT_STOP:
|
|
case OTA_OPCODE_TEST_STOP:
|
|
SeatDemoCompat_StopAllFromNative();
|
|
if(false != otaCalibrationDynamicActive)
|
|
{
|
|
OtaCalibration_StopDynamic(
|
|
OTA_CALIBRATION_ERROR_NONE, true);
|
|
}
|
|
else if(false != OtaCalibration_AnyInteractiveMoveActive())
|
|
{
|
|
OtaCalibration_FinalizeAllInteractiveMoves(
|
|
OTA_CALIBRATION_ERROR_NONE, true, true);
|
|
}
|
|
else if((false != otaSeatJobActive) ||
|
|
(otaCalibrationPendingJob != OTA_SEAT_JOB_NONE))
|
|
{
|
|
OtaCalibration_RequestAbort(OTA_CALIBRATION_ERROR_ABORTED);
|
|
otaCalibrationPendingJob = OTA_SEAT_JOB_NONE;
|
|
}
|
|
OtaCalibration_Respond(frame, OTA_STATUS_OK,
|
|
OtaSeat_JobStateValue());
|
|
break;
|
|
case OTA_OPCODE_TEST_INFO:
|
|
if(frame->data[3] == OTA_TEST_INFO_STATE)
|
|
{
|
|
OtaCalibration_Respond(frame, OTA_STATUS_OK,
|
|
OtaCalibration_TestState());
|
|
}
|
|
else
|
|
{
|
|
OtaCalibration_Respond(frame, OTA_STATUS_OK, 0ul);
|
|
}
|
|
break;
|
|
case OTA_OPCODE_ENTER_BOOTLOADER:
|
|
otaCalibrationPendingBootloader = true;
|
|
SeatDemoCompat_StopAllFromNative();
|
|
if(false != otaCalibrationDynamicActive)
|
|
{
|
|
OtaCalibration_StopDynamic(
|
|
OTA_CALIBRATION_ERROR_NONE, true);
|
|
}
|
|
else if(false != OtaCalibration_AnyInteractiveMoveActive())
|
|
{
|
|
OtaCalibration_FinalizeAllInteractiveMoves(
|
|
OTA_CALIBRATION_ERROR_NONE, true, true);
|
|
}
|
|
else
|
|
{
|
|
OtaCalibration_RequestAbort(OTA_CALIBRATION_ERROR_ABORTED);
|
|
}
|
|
OtaCalibration_Respond(frame, OTA_STATUS_OK,
|
|
OtaSeat_JobStateValue());
|
|
break;
|
|
default:
|
|
OtaCalibration_Respond(frame, OTA_STATUS_BAD_OPCODE, opcode);
|
|
break;
|
|
}
|
|
}
|
|
|
|
static void OtaCalibration_ServiceHook(void)
|
|
{
|
|
ota_can_control_frame_t frame;
|
|
|
|
if((false == otaCalibrationCanReady) ||
|
|
(false != otaCalibrationHookActive))
|
|
{
|
|
return;
|
|
}
|
|
otaCalibrationHookActive = true;
|
|
/* Calibration and homing remain board-owned jobs. Only interactive MOVE
|
|
* uses the explicit heartbeat lease serviced below. */
|
|
OtaCan_Service();
|
|
while(false != OtaCan_PopControl(&frame))
|
|
{
|
|
OtaCalibration_HandleControl(&frame);
|
|
OtaCan_Service();
|
|
}
|
|
OtaCalibration_UpdateProgress();
|
|
OtaCalibration_ServiceInteractiveMove();
|
|
OtaCalibration_ServiceDynamicMode();
|
|
|
|
if((false != otaSeatJobActive) &&
|
|
(otaSeatJobType != OTA_SEAT_JOB_MOVE) &&
|
|
(otaSeatJobType != OTA_SEAT_JOB_DYNAMIC))
|
|
{
|
|
if(false != driverFaultLatched)
|
|
{
|
|
OtaCalibration_RequestAbort(OTA_CALIBRATION_ERROR_DRIVER);
|
|
}
|
|
}
|
|
otaCalibrationHookActive = false;
|
|
}
|
|
|
|
static void OtaCalibration_SetFailed(ota_calibration_error_t fallback)
|
|
{
|
|
uint32_t axis;
|
|
|
|
if(otaSeatJobError == OTA_CALIBRATION_ERROR_NONE)
|
|
{
|
|
otaSeatJobError = (uint8_t)fallback;
|
|
}
|
|
for(axis = 0ul; axis < MOTOR_PROFILE_COUNT; axis++)
|
|
{
|
|
(void)Motor_StopMode((motor_mode_t)axis);
|
|
}
|
|
otaSeatJobActive = false;
|
|
otaSeatJobComplete = true;
|
|
otaSeatJobSuccess = false;
|
|
otaSeatJobPhase = OTA_SEAT_PHASE_FAILED;
|
|
}
|
|
|
|
static bool OtaCalibration_RunAxis(uint32_t axis,
|
|
ota_seat_job_type_t job)
|
|
{
|
|
motor_calibration_result_t result;
|
|
motor_safety_options_t safety = { false, false };
|
|
uint32_t rawBefore;
|
|
uint32_t rawAfter;
|
|
uint32_t returnPulses;
|
|
uint32_t previousRange;
|
|
uint32_t previousPosition;
|
|
uint32_t savedIntrStatus;
|
|
bool previousValid;
|
|
|
|
otaSeatJobAxis = (uint8_t)axis;
|
|
otaCalibrationPass = 0u;
|
|
otaSeatJobPhase = OTA_SEAT_PHASE_FIND_NEAR;
|
|
if((MOTOR_API_SUCCESS != Motor_SetSafetyOptions(
|
|
(motor_mode_t)axis, &safety)) ||
|
|
(MOTOR_API_SUCCESS != Motor_DisableSpeedControl((motor_mode_t)axis)))
|
|
{
|
|
otaSeatJobError = OTA_CALIBRATION_ERROR_MOTOR;
|
|
return false;
|
|
}
|
|
|
|
if(job == OTA_SEAT_JOB_HOME)
|
|
{
|
|
if(MOTOR_API_SUCCESS != Motor_Calibrate(
|
|
(motor_mode_t)axis, MOTOR_CALIBRATE_FIND_NEAR, &result))
|
|
{
|
|
if(false == otaCalibrationAbortRequested)
|
|
{
|
|
otaSeatJobError = OTA_CALIBRATION_ERROR_MOTOR;
|
|
}
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
savedIntrStatus = Cy_SysLib_EnterCriticalSection();
|
|
previousRange = motorRuntime[axis].maxHallCount;
|
|
previousPosition = motorRuntime[axis].positionHallCount;
|
|
previousValid = motorRuntime[axis].positionValid;
|
|
Cy_SysLib_ExitCriticalSection(savedIntrStatus);
|
|
|
|
if((MOTOR_API_SUCCESS != Motor_Calibrate(
|
|
(motor_mode_t)axis, MOTOR_CALIBRATE_FIND_BOTH, &result)) ||
|
|
(false == result.nearEndpointFound) ||
|
|
(false == result.farEndpointFound) ||
|
|
(result.measuredMaxHallCount < OTA_CALIBRATION_MINIMUM_PULSES) ||
|
|
(result.measuredMaxHallCount > 65535ul))
|
|
{
|
|
if(false == otaCalibrationAbortRequested)
|
|
{
|
|
otaSeatJobError = OTA_CALIBRATION_ERROR_RANGE;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
otaCalibrationPass = 1u;
|
|
otaSeatJobPhase = OTA_SEAT_PHASE_RETURN_NEAR;
|
|
savedIntrStatus = Cy_SysLib_EnterCriticalSection();
|
|
rawBefore = motorRuntime[axis].rawRisingEdges;
|
|
Cy_SysLib_ExitCriticalSection(savedIntrStatus);
|
|
if(MOTOR_API_SUCCESS != Motor_Calibrate(
|
|
(motor_mode_t)axis, MOTOR_CALIBRATE_FIND_NEAR, &result))
|
|
{
|
|
if(false == otaCalibrationAbortRequested)
|
|
{
|
|
otaSeatJobError = OTA_CALIBRATION_ERROR_MOTOR;
|
|
}
|
|
return false;
|
|
}
|
|
savedIntrStatus = Cy_SysLib_EnterCriticalSection();
|
|
rawAfter = motorRuntime[axis].rawRisingEdges;
|
|
Cy_SysLib_ExitCriticalSection(savedIntrStatus);
|
|
returnPulses = rawAfter - rawBefore;
|
|
if(false == OtaCalibration_ReturnMatches(
|
|
motorRuntime[axis].maxHallCount, returnPulses))
|
|
{
|
|
savedIntrStatus = Cy_SysLib_EnterCriticalSection();
|
|
motorRuntime[axis].maxHallCount = previousRange;
|
|
motorRuntime[axis].positionHallCount =
|
|
(false != previousValid) ? 0ul : previousPosition;
|
|
motorRuntime[axis].positionValid = previousValid;
|
|
Cy_SysLib_ExitCriticalSection(savedIntrStatus);
|
|
otaSeatJobError = OTA_CALIBRATION_ERROR_RETURN;
|
|
return false;
|
|
}
|
|
|
|
savedIntrStatus = Cy_SysLib_EnterCriticalSection();
|
|
motorRuntime[axis].positionHallCount = 0ul;
|
|
motorRuntime[axis].positionValid = true;
|
|
motorRuntime[axis].lastEvent = MOTOR_EVENT_NEAR_ENDPOINT;
|
|
Cy_SysLib_ExitCriticalSection(savedIntrStatus);
|
|
return true;
|
|
}
|
|
|
|
static void OtaCalibration_RunPendingJob(void)
|
|
{
|
|
ota_seat_job_type_t job = otaCalibrationPendingJob;
|
|
uint32_t axis;
|
|
bool success = true;
|
|
|
|
SeatDemoCompat_StopAllFromNative();
|
|
otaCalibrationPendingJob = OTA_SEAT_JOB_NONE;
|
|
otaCalibrationAbortRequested = false;
|
|
otaSeatJobActive = true;
|
|
otaSeatJobComplete = false;
|
|
otaSeatJobSuccess = false;
|
|
otaSeatJobType = job;
|
|
otaSeatJobPhase = OTA_SEAT_PHASE_FIND_NEAR;
|
|
otaSeatJobAxis = 0u;
|
|
otaSeatJobError = OTA_CALIBRATION_ERROR_NONE;
|
|
otaSeatJobProgress = 0u;
|
|
OtaCan_ClearLatchedFaults();
|
|
Cy_GPIO_Clr(DRV_DRVOFF_PORT, DRV_DRVOFF_PIN);
|
|
Cy_SysTick_DelayInUs(100ul);
|
|
OtaCalibration_Trace(0x00000010ul);
|
|
|
|
for(axis = 0ul; axis < MOTOR_PROFILE_COUNT; axis++)
|
|
{
|
|
otaSeatJobAxis = (uint8_t)axis;
|
|
OtaCalibration_Trace(0x00000020ul);
|
|
if((false != otaCalibrationAbortRequested) ||
|
|
(false == OtaCalibration_RunAxis(axis, job)))
|
|
{
|
|
success = false;
|
|
OtaCalibration_Trace(0x000000E1ul);
|
|
break;
|
|
}
|
|
OtaCalibration_Trace(0x00000021ul);
|
|
}
|
|
if(false == success)
|
|
{
|
|
OtaCalibration_SetFailed(
|
|
(false != otaCalibrationAbortRequested) ?
|
|
OTA_CALIBRATION_ERROR_ABORTED : OTA_CALIBRATION_ERROR_MOTOR);
|
|
OtaCalibration_Trace(0x000000EFul);
|
|
return;
|
|
}
|
|
|
|
/* Flash persistence is intentionally outside the per-axis loop. */
|
|
otaSeatJobAxis = 0x0Fu;
|
|
otaSeatJobProgress = 99u;
|
|
otaSeatJobPhase = OTA_SEAT_PHASE_SAVE;
|
|
OtaCalibration_Trace(0x00000030ul);
|
|
if(false == OtaSeat_SavePersistentState())
|
|
{
|
|
otaSeatJobError = OTA_CALIBRATION_ERROR_STORAGE;
|
|
OtaCalibration_SetFailed(OTA_CALIBRATION_ERROR_STORAGE);
|
|
OtaCalibration_Trace(0x000000E4ul);
|
|
return;
|
|
}
|
|
OtaCalibration_Trace(0x00000031ul);
|
|
|
|
otaSeatJobProgress = 100u;
|
|
otaSeatJobPhase = OTA_SEAT_PHASE_COMPLETE;
|
|
otaSeatJobActive = false;
|
|
otaSeatJobComplete = true;
|
|
otaSeatJobSuccess = true;
|
|
otaSeatJobError = OTA_CALIBRATION_ERROR_NONE;
|
|
OtaCalibration_Trace(0x000000FFul);
|
|
}
|
|
|
|
static void OtaCalibration_EnterBootloaderNow(void)
|
|
{
|
|
uint32_t waitMs;
|
|
|
|
SeatDemoCompat_PrepareForBootloader();
|
|
(void)OtaSeat_SavePersistentState();
|
|
for(waitMs = 0ul; waitMs < 100ul; waitMs++)
|
|
{
|
|
OtaCan_Service();
|
|
if(false != OtaCan_TxIdle()) break;
|
|
Cy_SysTick_DelayInUs(1000ul);
|
|
}
|
|
Ota_EnterBootloader();
|
|
}
|
|
|
|
static int OtaCalibration_Run(bool driverInitOk)
|
|
{
|
|
g_otaCalibrationTrace.magic = OTA_CALIBRATION_TRACE_MAGIC;
|
|
g_otaCalibrationTrace.nextSequence = 0ul;
|
|
otaSeatLastSaveOk = true;
|
|
OtaSeat_RestorePersistentState();
|
|
OtaCan_DebugStart();
|
|
if((false == driverInitOk) || (false == OtaCan_Init()))
|
|
{
|
|
for(;;)
|
|
{
|
|
Cy_GPIO_Set(DRV_DRVOFF_PORT, DRV_DRVOFF_PIN);
|
|
}
|
|
}
|
|
otaCalibrationCanReady = true;
|
|
SeatDemoCompat_Init();
|
|
|
|
for(;;)
|
|
{
|
|
Motor_ServiceAll();
|
|
if(otaCalibrationPendingJob != OTA_SEAT_JOB_NONE)
|
|
{
|
|
OtaCalibration_RunPendingJob();
|
|
}
|
|
if((false != otaCalibrationPendingBootloader) &&
|
|
(false == otaSeatJobActive) &&
|
|
(otaCalibrationMoveSavePendingMask == 0ul))
|
|
{
|
|
OtaCalibration_EnterBootloaderNow();
|
|
}
|
|
__WFI();
|
|
}
|
|
}
|