Import independent seat firmware source without build artifacts
This commit is contained in:
Binary file not shown.
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,28 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<iarUserArgVars>
|
||||
<group name="CUSTOM_VARS" active="true">
|
||||
<variable>
|
||||
<name>LINK_LOCATION</name>
|
||||
<value>flash</value>
|
||||
</variable>
|
||||
<variable>
|
||||
<name>MULTICORE_DEBUG_SLAVE_WORKSPACE</name>
|
||||
<value></value>
|
||||
</variable>
|
||||
<variable>
|
||||
<name>SDK_ROOT</name>
|
||||
<value>..\..\..\third_party\ZL-test-scripts\T2G_Sample_Driver_Library_7.9.0</value>
|
||||
</variable>
|
||||
<variable>
|
||||
<name>SOURCE_ROOT</name>
|
||||
<value>..\..\..\third_party\ZL-test-scripts</value>
|
||||
</variable>
|
||||
<variable><name>MOTOR_TEST_SCENARIO</name><value>0</value></variable>
|
||||
<variable><name>MOTOR_TEST_MODE</name><value>0</value></variable>
|
||||
<variable><name>MOTOR_TEST_TARGET_X100</name><value>0</value></variable>
|
||||
<variable><name>MOTOR_TEST_DURATION_MS</name><value>0</value></variable>
|
||||
<variable><name>MOTOR_TEST_DIRECTION</name><value>0</value></variable>
|
||||
<variable><name>MOTOR_TEST_BUILD_KEY</name><value>0</value></variable>
|
||||
<variable><name>MOTOR_OTA_INTERACTIVE_TEST</name><value>0</value></variable>
|
||||
</group>
|
||||
</iarUserArgVars>
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,18 @@
|
||||
#include "cy_project.h"
|
||||
#include "cy_device_headers.h"
|
||||
|
||||
#include "../common/ota_protocol.h"
|
||||
#include "ota_enter_bootloader.h"
|
||||
|
||||
void Ota_EnterBootloader(void)
|
||||
{
|
||||
__disable_irq();
|
||||
BACKUP->unBREG[0].u32Register = OTA_BOOT_REQUEST_MAGIC;
|
||||
BACKUP->unBREG[1].u32Register = OTA_BOOT_REQUEST_MAGIC_INVERSE;
|
||||
__DSB();
|
||||
NVIC_SystemReset();
|
||||
for(;;)
|
||||
{
|
||||
/* Reset must not return. */
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,7 @@
|
||||
#ifndef OTA_ENTER_BOOTLOADER_H
|
||||
#define OTA_ENTER_BOOTLOADER_H
|
||||
|
||||
/* Optional application hook: call this after stopping all motors safely. */
|
||||
void Ota_EnterBootloader(void);
|
||||
|
||||
#endif /* OTA_ENTER_BOOTLOADER_H */
|
||||
@@ -0,0 +1,306 @@
|
||||
/* Included into the generated main_cm0plus.c for the eight Android-controlled
|
||||
* direction-test images. It intentionally lives in the same translation unit
|
||||
* so it can reuse the proven static motor safety/event implementation without
|
||||
* changing the bundled third_party reference source. */
|
||||
|
||||
static bool otaInteractiveDriverInitOk;
|
||||
static bool otaInteractiveFinalized = true;
|
||||
|
||||
static void OtaInteractive_ClearResult(void)
|
||||
{
|
||||
simpleDirectionResult.risingEdges = 0ul;
|
||||
simpleDirectionResult.totalEdges = 0ul;
|
||||
simpleDirectionResult.activeMs = 0ul;
|
||||
simpleDirectionResult.safetyWindowPulses = 0ul;
|
||||
simpleDirectionResult.driverStatus = 0u;
|
||||
simpleDirectionResult.driverFaults = 0u;
|
||||
simpleDirectionResult.finalHallLevel = 0u;
|
||||
simpleDirectionResult.stopReason =
|
||||
SIMPLE_DIRECTION_STOP_REASON_NOT_STARTED;
|
||||
simpleDirectionResult.armed = false;
|
||||
simpleDirectionResult.prepared = false;
|
||||
simpleDirectionResult.timingValid = false;
|
||||
simpleDirectionResult.diagnosticsValid = false;
|
||||
simpleDirectionRunActive = false;
|
||||
simpleDirectionRunComplete = false;
|
||||
}
|
||||
|
||||
static void OtaInteractive_Finalize(void)
|
||||
{
|
||||
uint8_t driverStatus = 0u;
|
||||
bool statusReadOk;
|
||||
bool stopOk;
|
||||
|
||||
if((false == simpleDirectionRunComplete) ||
|
||||
(false != otaInteractiveFinalized))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
Motor_ServiceAll();
|
||||
statusReadOk = Drv8718_ReadRegRaw(DRV8718_REG_IC_STAT1,
|
||||
&driverStatus);
|
||||
if(false != statusReadOk)
|
||||
{
|
||||
simpleDirectionResult.driverStatus = driverStatus;
|
||||
simpleDirectionResult.driverFaults =
|
||||
(uint16_t)(driverStatus & DRV8718_IC_STAT1_ALERT_MASK);
|
||||
}
|
||||
stopOk = (MOTOR_API_SUCCESS ==
|
||||
Motor_StopMode((motor_mode_t)MOTOR_AUTOMATED_TEST_MODE));
|
||||
if(false != stopOk)
|
||||
{
|
||||
stopOk = Motor_WaitForCoastToSettle(
|
||||
(motor_mode_t)MOTOR_AUTOMATED_TEST_MODE);
|
||||
}
|
||||
simpleDirectionResult.diagnosticsValid = statusReadOk && stopOk;
|
||||
SimpleDirectionTest_ImmediateAllOff();
|
||||
(void)OtaSeat_SavePersistentState();
|
||||
otaInteractiveFinalized = true;
|
||||
}
|
||||
|
||||
static void OtaInteractive_Stop(void)
|
||||
{
|
||||
uint32_t savedIntrStatus;
|
||||
|
||||
if(false != simpleDirectionRunActive)
|
||||
{
|
||||
savedIntrStatus = Cy_SysLib_EnterCriticalSection();
|
||||
Motor_RequestEventFromIsr(MOTOR_AUTOMATED_TEST_MODE,
|
||||
MOTOR_EVENT_ABORTED);
|
||||
Cy_SysLib_ExitCriticalSection(savedIntrStatus);
|
||||
}
|
||||
if((false == simpleDirectionRunComplete) &&
|
||||
(false == otaInteractiveFinalized))
|
||||
{
|
||||
SimpleDirectionTest_ImmediateAllOff();
|
||||
SimpleDirectionTest_CaptureStopFromIsr(
|
||||
(uint32_t)(hallTimebaseTicks - simpleDirectionStartTicks),
|
||||
Hall_ReadLevel(MOTOR_AUTOMATED_TEST_MODE),
|
||||
SIMPLE_DIRECTION_STOP_REASON_ABORTED,
|
||||
motorRuntime[MOTOR_AUTOMATED_TEST_MODE].recentWindowPulses);
|
||||
}
|
||||
OtaInteractive_Finalize();
|
||||
}
|
||||
|
||||
static bool OtaInteractive_Start(void)
|
||||
{
|
||||
if((false != simpleDirectionRunActive) ||
|
||||
(false != driverFaultLatched))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
OtaInteractive_ClearResult();
|
||||
simpleDirectionResult.armed = true;
|
||||
simpleDirectionResult.prepared =
|
||||
SimpleDirectionTest_Prepare(otaInteractiveDriverInitOk);
|
||||
if(false == simpleDirectionResult.prepared)
|
||||
{
|
||||
simpleDirectionResult.prepared = false;
|
||||
SimpleDirectionTest_ImmediateAllOff();
|
||||
return false;
|
||||
}
|
||||
|
||||
/* OtaInteractiveTest_Run() and every completed/stopped test use
|
||||
* SimpleDirectionTest_ImmediateAllOff(), which asserts the external
|
||||
* DRVOFF pin. The original one-shot direction test never starts again
|
||||
* after that call, but an interactive OTA image must explicitly release
|
||||
* DRVOFF before each new TEST_START. Without this transition the SPI
|
||||
* bridge/PWM setup succeeds while the DRV8718 power outputs remain
|
||||
* hardware-disabled (IC_STAT1 still reads 0x80 and Hall stays at zero). */
|
||||
Cy_GPIO_Clr(DRV_DRVOFF_PORT, DRV_DRVOFF_PIN);
|
||||
Cy_SysTick_DelayInUs(100ul);
|
||||
if(false == SimpleDirectionTest_Start())
|
||||
{
|
||||
simpleDirectionResult.prepared = false;
|
||||
SimpleDirectionTest_ImmediateAllOff();
|
||||
return false;
|
||||
}
|
||||
otaInteractiveFinalized = false;
|
||||
return true;
|
||||
}
|
||||
|
||||
static uint32_t OtaInteractive_StateValue(void)
|
||||
{
|
||||
uint32_t value =
|
||||
((uint32_t)simpleDirectionResult.stopReason <<
|
||||
OTA_TEST_STATE_STOP_REASON_SHIFT) |
|
||||
((uint32_t)MOTOR_AUTOMATED_TEST_MODE <<
|
||||
OTA_TEST_STATE_MODE_SHIFT) |
|
||||
((uint32_t)MOTOR_AUTOMATED_TEST_DIRECTION <<
|
||||
OTA_TEST_STATE_DIRECTION_SHIFT);
|
||||
|
||||
if(false != simpleDirectionRunActive)
|
||||
{
|
||||
value |= OTA_TEST_STATE_ACTIVE;
|
||||
}
|
||||
if(false != simpleDirectionRunComplete)
|
||||
{
|
||||
value |= OTA_TEST_STATE_COMPLETE;
|
||||
}
|
||||
if(false != driverFaultLatched)
|
||||
{
|
||||
value |= OTA_TEST_STATE_DRIVER_FAULT;
|
||||
}
|
||||
if(false != simpleDirectionResult.prepared)
|
||||
{
|
||||
value |= OTA_TEST_STATE_PREPARED;
|
||||
}
|
||||
if(false != simpleDirectionResult.timingValid)
|
||||
{
|
||||
value |= OTA_TEST_STATE_TIMING_VALID;
|
||||
}
|
||||
if(false != simpleDirectionResult.diagnosticsValid)
|
||||
{
|
||||
value |= OTA_TEST_STATE_DIAGNOSTICS_VALID;
|
||||
}
|
||||
return value;
|
||||
}
|
||||
|
||||
static uint32_t OtaInteractive_InfoValue(uint8_t selector)
|
||||
{
|
||||
switch((ota_test_info_selector_t)selector)
|
||||
{
|
||||
case OTA_TEST_INFO_STATE:
|
||||
return OtaInteractive_StateValue();
|
||||
case OTA_TEST_INFO_RISING_EDGES:
|
||||
return simpleDirectionResult.risingEdges;
|
||||
case OTA_TEST_INFO_TOTAL_EDGES:
|
||||
return simpleDirectionResult.totalEdges;
|
||||
case OTA_TEST_INFO_ACTIVE_MS:
|
||||
return simpleDirectionResult.activeMs;
|
||||
case OTA_TEST_INFO_SAFETY_WINDOW_PULSES:
|
||||
return simpleDirectionResult.safetyWindowPulses;
|
||||
case OTA_TEST_INFO_DRIVER_DIAGNOSTICS:
|
||||
return ((uint32_t)simpleDirectionResult.driverFaults << 16u) |
|
||||
(uint32_t)simpleDirectionResult.driverStatus;
|
||||
default:
|
||||
return 0ul;
|
||||
}
|
||||
}
|
||||
|
||||
static void OtaInteractive_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 void OtaInteractive_EnterBootloader(
|
||||
const ota_can_control_frame_t *frame)
|
||||
{
|
||||
uint32_t waitMs;
|
||||
|
||||
OtaInteractive_Stop();
|
||||
(void)OtaSeat_SavePersistentState();
|
||||
OtaInteractive_Respond(frame, OTA_STATUS_OK,
|
||||
OtaInteractive_StateValue());
|
||||
for(waitMs = 0ul; waitMs < 100ul; waitMs++)
|
||||
{
|
||||
OtaCan_Service();
|
||||
if(false != OtaCan_TxIdle())
|
||||
{
|
||||
break;
|
||||
}
|
||||
Cy_SysTick_DelayInUs(1000ul);
|
||||
}
|
||||
SimpleDirectionTest_ImmediateAllOff();
|
||||
Ota_EnterBootloader();
|
||||
}
|
||||
|
||||
static void OtaInteractive_HandleControl(
|
||||
const ota_can_control_frame_t *frame)
|
||||
{
|
||||
uint8_t opcode = frame->data[2];
|
||||
|
||||
if(frame->data[0] != OTA_CONTROL_MAGIC)
|
||||
{
|
||||
OtaInteractive_Respond(frame, OTA_STATUS_BAD_MAGIC, 0ul);
|
||||
return;
|
||||
}
|
||||
|
||||
if(false != OtaSeat_HandleInfoControl(frame))
|
||||
{
|
||||
return;
|
||||
}
|
||||
switch((ota_opcode_t)opcode)
|
||||
{
|
||||
case OTA_OPCODE_TEST_INFO:
|
||||
if(frame->data[3] > OTA_TEST_INFO_DRIVER_DIAGNOSTICS)
|
||||
{
|
||||
OtaInteractive_Respond(frame, OTA_STATUS_BAD_ARGUMENT,
|
||||
frame->data[3]);
|
||||
}
|
||||
else
|
||||
{
|
||||
OtaInteractive_Respond(
|
||||
frame, OTA_STATUS_OK,
|
||||
OtaInteractive_InfoValue(frame->data[3]));
|
||||
}
|
||||
break;
|
||||
case OTA_OPCODE_TEST_START:
|
||||
if(false != OtaInteractive_Start())
|
||||
{
|
||||
OtaInteractive_Respond(frame, OTA_STATUS_OK,
|
||||
OtaInteractive_StateValue());
|
||||
}
|
||||
else
|
||||
{
|
||||
OtaInteractive_Respond(frame, OTA_STATUS_BAD_STATE,
|
||||
OtaInteractive_StateValue());
|
||||
}
|
||||
break;
|
||||
case OTA_OPCODE_TEST_STOP:
|
||||
OtaInteractive_Stop();
|
||||
OtaInteractive_Respond(frame, OTA_STATUS_OK,
|
||||
OtaInteractive_StateValue());
|
||||
break;
|
||||
case OTA_OPCODE_ENTER_BOOTLOADER:
|
||||
OtaInteractive_EnterBootloader(frame);
|
||||
break;
|
||||
default:
|
||||
OtaInteractive_Respond(frame, OTA_STATUS_BAD_OPCODE, opcode);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
static int OtaInteractiveTest_Run(bool driverInitOk)
|
||||
{
|
||||
ota_can_control_frame_t frame;
|
||||
|
||||
/* Keep the original one-shot runner linked for source-level parity; the
|
||||
* interactive build intentionally dispatches through this loop instead. */
|
||||
(void)SimpleDirectionTest_Run;
|
||||
otaInteractiveDriverInitOk = driverInitOk;
|
||||
OtaInteractive_ClearResult();
|
||||
OtaSeat_RestorePersistentState();
|
||||
SimpleDirectionTest_ImmediateAllOff();
|
||||
OtaCan_DebugStart();
|
||||
if(false == OtaCan_Init())
|
||||
{
|
||||
for(;;)
|
||||
{
|
||||
SimpleDirectionTest_ImmediateAllOff();
|
||||
}
|
||||
}
|
||||
|
||||
for(;;)
|
||||
{
|
||||
Motor_ServiceAll();
|
||||
OtaCan_Service();
|
||||
|
||||
if((false != simpleDirectionRunComplete) &&
|
||||
(false == otaInteractiveFinalized))
|
||||
{
|
||||
OtaInteractive_Finalize();
|
||||
}
|
||||
while(false != OtaCan_PopControl(&frame))
|
||||
{
|
||||
OtaInteractive_HandleControl(&frame);
|
||||
OtaCan_Service();
|
||||
}
|
||||
__WFI();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,244 @@
|
||||
/* Shared board-authoritative position/range state for every OTA application.
|
||||
* This file is included after the motor implementation so it can snapshot and
|
||||
* restore the proven per-axis runtime without changing the bundled source. */
|
||||
|
||||
static bool otaSeatStateLoaded;
|
||||
static bool otaSeatLastSaveOk;
|
||||
static uint32_t otaSeatPersistedMask;
|
||||
static uint32_t otaSeatStateSequence;
|
||||
static bool otaSeatJobActive;
|
||||
static bool otaSeatJobComplete;
|
||||
static bool otaSeatJobSuccess;
|
||||
static ota_seat_job_type_t otaSeatJobType = OTA_SEAT_JOB_NONE;
|
||||
static ota_seat_job_phase_t otaSeatJobPhase = OTA_SEAT_PHASE_IDLE;
|
||||
static uint8_t otaSeatJobAxis = 0x0Fu;
|
||||
static uint8_t otaSeatJobError;
|
||||
static uint8_t otaSeatJobProgress;
|
||||
|
||||
static bool OtaSeat_SavePersistentState(void);
|
||||
|
||||
static uint32_t OtaSeat_RuntimeKind(void)
|
||||
{
|
||||
#if (MOTOR_OTA_CALIBRATION != 0u)
|
||||
return OTA_SEAT_RUNTIME_CALIBRATION;
|
||||
#elif (MOTOR_OTA_SELF_COMPOSE != 0u)
|
||||
return OTA_SEAT_RUNTIME_DUAL_PROTOCOL;
|
||||
#else
|
||||
return OTA_SEAT_RUNTIME_FBL;
|
||||
#endif
|
||||
}
|
||||
|
||||
static void OtaSeat_RestorePersistentState(void)
|
||||
{
|
||||
ota_seat_state_data_t stored;
|
||||
uint32_t axis;
|
||||
uint32_t savedIntrStatus;
|
||||
bool normalizePersistentState = false;
|
||||
|
||||
otaSeatStateLoaded = OtaSeatState_Load(&stored);
|
||||
otaSeatLastSaveOk = otaSeatStateLoaded;
|
||||
otaSeatPersistedMask = 0ul;
|
||||
otaSeatStateSequence = 0ul;
|
||||
if(false == otaSeatStateLoaded)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
savedIntrStatus = Cy_SysLib_EnterCriticalSection();
|
||||
otaSeatStateSequence = stored.sequence;
|
||||
otaSeatPersistedMask = 0ul;
|
||||
for(axis = 0ul; axis < MOTOR_PROFILE_COUNT; axis++)
|
||||
{
|
||||
uint32_t defaultRange = motorProfiles[axis].defaultMaxHallCount;
|
||||
motorRuntime[axis].maxHallCount = defaultRange;
|
||||
if((stored.validMask & (1ul << axis)) != 0ul)
|
||||
{
|
||||
uint32_t position = stored.position[axis];
|
||||
|
||||
/* The Base owns all four physical Hall ranges. Persistent storage
|
||||
* remembers position only and can never override a Base range. */
|
||||
if(stored.range[axis] != defaultRange)
|
||||
{
|
||||
normalizePersistentState = true;
|
||||
}
|
||||
if(position > defaultRange)
|
||||
{
|
||||
position = defaultRange;
|
||||
normalizePersistentState = true;
|
||||
}
|
||||
|
||||
motorRuntime[axis].positionHallCount = position;
|
||||
motorRuntime[axis].positionValid = true;
|
||||
otaSeatPersistedMask |= 1ul << axis;
|
||||
}
|
||||
}
|
||||
Cy_SysLib_ExitCriticalSection(savedIntrStatus);
|
||||
|
||||
if(false != normalizePersistentState)
|
||||
{
|
||||
(void)OtaSeat_SavePersistentState();
|
||||
}
|
||||
}
|
||||
|
||||
static bool OtaSeat_SavePersistentState(void)
|
||||
{
|
||||
ota_seat_state_data_t state;
|
||||
uint32_t axis;
|
||||
uint32_t savedIntrStatus;
|
||||
|
||||
state.sequence = otaSeatStateSequence + 1ul;
|
||||
state.validMask = 0ul;
|
||||
savedIntrStatus = Cy_SysLib_EnterCriticalSection();
|
||||
for(axis = 0ul; axis < MOTOR_PROFILE_COUNT; axis++)
|
||||
{
|
||||
uint32_t range = motorProfiles[axis].defaultMaxHallCount;
|
||||
uint32_t position = motorRuntime[axis].positionHallCount;
|
||||
motorRuntime[axis].maxHallCount = range;
|
||||
if(position > range)
|
||||
{
|
||||
position = range;
|
||||
}
|
||||
state.range[axis] = range;
|
||||
state.position[axis] = position;
|
||||
if(false != motorRuntime[axis].positionValid)
|
||||
{
|
||||
state.validMask |= 1ul << axis;
|
||||
}
|
||||
}
|
||||
Cy_SysLib_ExitCriticalSection(savedIntrStatus);
|
||||
|
||||
otaSeatLastSaveOk = OtaSeatState_Save(&state);
|
||||
if(false != otaSeatLastSaveOk)
|
||||
{
|
||||
otaSeatStateLoaded = true;
|
||||
otaSeatStateSequence = state.sequence;
|
||||
otaSeatPersistedMask = state.validMask;
|
||||
}
|
||||
return otaSeatLastSaveOk;
|
||||
}
|
||||
|
||||
static uint32_t OtaSeat_JobStateValue(void)
|
||||
{
|
||||
uint32_t value =
|
||||
((uint32_t)otaSeatJobType << OTA_SEAT_JOB_TYPE_SHIFT) |
|
||||
((uint32_t)otaSeatJobPhase << OTA_SEAT_JOB_PHASE_SHIFT) |
|
||||
((uint32_t)otaSeatJobAxis << OTA_SEAT_JOB_AXIS_SHIFT) |
|
||||
((uint32_t)otaSeatJobError << OTA_SEAT_JOB_ERROR_SHIFT) |
|
||||
((uint32_t)otaSeatJobProgress << OTA_SEAT_JOB_PROGRESS_SHIFT);
|
||||
|
||||
if(false != otaSeatJobActive) value |= OTA_SEAT_JOB_ACTIVE;
|
||||
if(false != otaSeatJobComplete) value |= OTA_SEAT_JOB_COMPLETE;
|
||||
if(false != otaSeatJobSuccess) value |= OTA_SEAT_JOB_SUCCESS;
|
||||
if(false != otaSeatLastSaveOk) value |= OTA_SEAT_JOB_STORAGE_OK;
|
||||
return value;
|
||||
}
|
||||
|
||||
static uint32_t OtaSeat_AxisPositionRangeValue(uint32_t axis)
|
||||
{
|
||||
uint32_t position;
|
||||
uint32_t range;
|
||||
uint32_t savedIntrStatus;
|
||||
|
||||
savedIntrStatus = Cy_SysLib_EnterCriticalSection();
|
||||
position = motorRuntime[axis].positionHallCount;
|
||||
range = motorRuntime[axis].maxHallCount;
|
||||
Cy_SysLib_ExitCriticalSection(savedIntrStatus);
|
||||
if(position > 65535ul) position = 65535ul;
|
||||
if(range > 65535ul) range = 65535ul;
|
||||
return (range << 16u) | position;
|
||||
}
|
||||
|
||||
static uint32_t OtaSeat_AxisFlagsValue(uint32_t axis)
|
||||
{
|
||||
uint32_t value;
|
||||
uint32_t savedIntrStatus;
|
||||
|
||||
savedIntrStatus = Cy_SysLib_EnterCriticalSection();
|
||||
value = ((uint32_t)motorRuntime[axis].direction <<
|
||||
OTA_SEAT_AXIS_DIRECTION_SHIFT) |
|
||||
((uint32_t)motorRuntime[axis].lastEvent <<
|
||||
OTA_SEAT_AXIS_EVENT_SHIFT);
|
||||
if(false != motorRuntime[axis].positionValid)
|
||||
{
|
||||
value |= OTA_SEAT_AXIS_VALID;
|
||||
}
|
||||
if((motorRuntime[axis].direction != MOTOR_DIRECTION_STOPPED) ||
|
||||
(false != motorRuntime[axis].startPending))
|
||||
{
|
||||
value |= OTA_SEAT_AXIS_MOVING;
|
||||
}
|
||||
if(false != motorRuntime[axis].calibrating)
|
||||
{
|
||||
value |= OTA_SEAT_AXIS_CALIBRATING;
|
||||
}
|
||||
Cy_SysLib_ExitCriticalSection(savedIntrStatus);
|
||||
if(false != driverFaultLatched) value |= OTA_SEAT_AXIS_DRIVER_FAULT;
|
||||
if((otaSeatPersistedMask & (1ul << axis)) != 0ul)
|
||||
{
|
||||
value |= OTA_SEAT_AXIS_PERSISTED;
|
||||
}
|
||||
return value;
|
||||
}
|
||||
|
||||
static uint32_t OtaSeat_InfoValue(uint8_t selector)
|
||||
{
|
||||
uint32_t axis;
|
||||
|
||||
if(selector == OTA_SEAT_INFO_JOB)
|
||||
{
|
||||
return OtaSeat_JobStateValue();
|
||||
}
|
||||
if((selector >= OTA_SEAT_INFO_AXIS0_POSITION_RANGE) &&
|
||||
(selector <= OTA_SEAT_INFO_AXIS3_POSITION_RANGE))
|
||||
{
|
||||
axis = (uint32_t)(selector - OTA_SEAT_INFO_AXIS0_POSITION_RANGE);
|
||||
return OtaSeat_AxisPositionRangeValue(axis);
|
||||
}
|
||||
if((selector >= OTA_SEAT_INFO_AXIS0_FLAGS) &&
|
||||
(selector <= OTA_SEAT_INFO_AXIS3_FLAGS))
|
||||
{
|
||||
axis = (uint32_t)(selector - OTA_SEAT_INFO_AXIS0_FLAGS);
|
||||
return OtaSeat_AxisFlagsValue(axis);
|
||||
}
|
||||
if((selector >= OTA_SEAT_INFO_AXIS0_PHYSICAL_X100) &&
|
||||
(selector <= OTA_SEAT_INFO_AXIS3_PHYSICAL_X100))
|
||||
{
|
||||
axis = (uint32_t)(selector - OTA_SEAT_INFO_AXIS0_PHYSICAL_X100);
|
||||
return motorProfiles[axis].maxPhysicalValueX100;
|
||||
}
|
||||
if(selector == OTA_SEAT_INFO_STORAGE)
|
||||
{
|
||||
return (otaSeatPersistedMask & 0x0Ful) |
|
||||
((false != otaSeatStateLoaded) ? (1ul << 8u) : 0ul) |
|
||||
((false != otaSeatLastSaveOk) ? (1ul << 9u) : 0ul) |
|
||||
((otaSeatStateSequence & 0xFFFFul) << 16u);
|
||||
}
|
||||
if(selector == OTA_SEAT_INFO_RUNTIME_KIND)
|
||||
{
|
||||
return OtaSeat_RuntimeKind();
|
||||
}
|
||||
return 0ul;
|
||||
}
|
||||
|
||||
static bool OtaSeat_HandleInfoControl(const ota_can_control_frame_t *frame)
|
||||
{
|
||||
uint8_t selector;
|
||||
|
||||
if(frame->data[2] != OTA_OPCODE_SEAT_INFO)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
selector = frame->data[3];
|
||||
if(selector > OTA_SEAT_INFO_RUNTIME_KIND)
|
||||
{
|
||||
(void)OtaCan_QueueResponse(frame->data[1], frame->data[2],
|
||||
OTA_STATUS_BAD_ARGUMENT, selector);
|
||||
}
|
||||
else
|
||||
{
|
||||
(void)OtaCan_QueueResponse(frame->data[1], frame->data[2],
|
||||
OTA_STATUS_OK,
|
||||
OtaSeat_InfoValue(selector));
|
||||
}
|
||||
return true;
|
||||
}
|
||||
@@ -0,0 +1,225 @@
|
||||
#include "cy_project.h"
|
||||
#include "cy_device_headers.h"
|
||||
#include "flash/cy_flash.h"
|
||||
|
||||
#include "ota_memory_map.h"
|
||||
#include "ota_seat_state.h"
|
||||
|
||||
#include <stddef.h>
|
||||
|
||||
#define OTA_SEAT_STATE_LEGACY_VERSION (1ul)
|
||||
#define OTA_SEAT_STATE_REVERSED_DIRECTION_AXIS (1ul)
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint32_t magic;
|
||||
uint32_t formatVersion;
|
||||
uint32_t sequence;
|
||||
uint32_t validMask;
|
||||
uint32_t range[OTA_SEAT_AXIS_COUNT];
|
||||
uint32_t position[OTA_SEAT_AXIS_COUNT];
|
||||
uint32_t crc32;
|
||||
} ota_seat_state_record_t;
|
||||
|
||||
static const uint32_t otaSeatFixedHallRange[OTA_SEAT_AXIS_COUNT] =
|
||||
{
|
||||
OTA_SEAT_AXIS0_FIXED_HALL_RANGE,
|
||||
OTA_SEAT_AXIS1_FIXED_HALL_RANGE,
|
||||
OTA_SEAT_AXIS2_FIXED_HALL_RANGE,
|
||||
OTA_SEAT_AXIS3_FIXED_HALL_RANGE
|
||||
};
|
||||
|
||||
static uint32_t OtaSeatState_Crc32(const uint8_t *data, uint32_t length)
|
||||
{
|
||||
uint32_t crc = 0xFFFFFFFFul;
|
||||
uint32_t index;
|
||||
uint8_t bit;
|
||||
|
||||
for(index = 0ul; index < length; index++)
|
||||
{
|
||||
crc ^= (uint32_t)data[index];
|
||||
for(bit = 0u; bit < 8u; bit++)
|
||||
{
|
||||
crc = ((crc & 1ul) != 0ul) ?
|
||||
((crc >> 1u) ^ 0xEDB88320ul) : (crc >> 1u);
|
||||
}
|
||||
}
|
||||
return crc ^ 0xFFFFFFFFul;
|
||||
}
|
||||
|
||||
static bool OtaSeatState_EraseSector(void)
|
||||
{
|
||||
cy_stc_flash_erasesector_config_t config;
|
||||
|
||||
config.Addr = (const uint32_t *)OTA_SEAT_STATE_ADDRESS;
|
||||
config.blocking = CY_FLASH_ERASESECTOR_BLOCKING;
|
||||
config.intrMask = CY_FLASH_ERASESECTOR_NOT_SET_INTR_MASK;
|
||||
return Cy_Flash_EraseSector(NULL, &config, CY_FLASH_DRIVER_BLOCKING) ==
|
||||
CY_FLASH_DRV_SUCCESS;
|
||||
}
|
||||
|
||||
static bool OtaSeatState_ProgramWord(uint32_t address, const uint32_t *word)
|
||||
{
|
||||
cy_stc_flash_programrow_config_t config;
|
||||
|
||||
config.destAddr = (const uint32_t *)address;
|
||||
config.dataAddr = word;
|
||||
config.blocking = CY_FLASH_PROGRAMROW_BLOCKING;
|
||||
config.skipBC = CY_FLASH_PROGRAMROW_SKIP_BLANK_CHECK;
|
||||
config.dataSize = CY_FLASH_PROGRAMROW_DATA_SIZE_32BIT;
|
||||
config.dataLoc = CY_FLASH_PROGRAMROW_DATA_LOCATION_SRAM;
|
||||
config.intrMask = CY_FLASH_PROGRAMROW_NOT_SET_INTR_MASK;
|
||||
return Cy_Flash_ProgramRow(NULL, &config, CY_FLASH_DRIVER_BLOCKING) ==
|
||||
CY_FLASH_DRV_SUCCESS;
|
||||
}
|
||||
|
||||
static bool OtaSeatState_RecordValid(const ota_seat_state_record_t *record)
|
||||
{
|
||||
uint32_t axis;
|
||||
uint32_t expected;
|
||||
|
||||
if((NULL == record) || (record->magic != OTA_SEAT_STATE_MAGIC) ||
|
||||
((record->formatVersion != OTA_SEAT_STATE_VERSION) &&
|
||||
(record->formatVersion != OTA_SEAT_STATE_LEGACY_VERSION)) ||
|
||||
((record->validMask & ~0x0Ful) != 0ul))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
expected = OtaSeatState_Crc32(
|
||||
(const uint8_t *)record,
|
||||
(uint32_t)offsetof(ota_seat_state_record_t, crc32));
|
||||
if(record->crc32 != expected)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
for(axis = 0ul; axis < OTA_SEAT_AXIS_COUNT; axis++)
|
||||
{
|
||||
if(((record->validMask & (1ul << axis)) != 0ul) &&
|
||||
((record->range[axis] < 32ul) ||
|
||||
(record->range[axis] > 65535ul) ||
|
||||
(record->position[axis] > record->range[axis])))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool OtaSeatState_Load(ota_seat_state_data_t *state)
|
||||
{
|
||||
const volatile ota_seat_state_record_t *stored =
|
||||
(const volatile ota_seat_state_record_t *)OTA_SEAT_STATE_ADDRESS;
|
||||
ota_seat_state_record_t record;
|
||||
uint32_t axis;
|
||||
|
||||
if(NULL == state)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
Cy_Flashc_InvalidateFlashCacheBuffer();
|
||||
record.magic = stored->magic;
|
||||
record.formatVersion = stored->formatVersion;
|
||||
record.sequence = stored->sequence;
|
||||
record.validMask = stored->validMask;
|
||||
for(axis = 0ul; axis < OTA_SEAT_AXIS_COUNT; axis++)
|
||||
{
|
||||
record.range[axis] = stored->range[axis];
|
||||
record.position[axis] = stored->position[axis];
|
||||
}
|
||||
record.crc32 = stored->crc32;
|
||||
if(false == OtaSeatState_RecordValid(&record))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
state->sequence = record.sequence;
|
||||
state->validMask = record.validMask;
|
||||
for(axis = 0ul; axis < OTA_SEAT_AXIS_COUNT; axis++)
|
||||
{
|
||||
uint32_t position = record.position[axis];
|
||||
state->range[axis] = otaSeatFixedHallRange[axis];
|
||||
if(position > otaSeatFixedHallRange[axis])
|
||||
{
|
||||
position = otaSeatFixedHallRange[axis];
|
||||
}
|
||||
state->position[axis] = position;
|
||||
}
|
||||
if((record.formatVersion == OTA_SEAT_STATE_LEGACY_VERSION) &&
|
||||
((record.validMask &
|
||||
(1ul << OTA_SEAT_STATE_REVERSED_DIRECTION_AXIS)) != 0ul))
|
||||
{
|
||||
/* Version 1 used the opposite electrical near/far polarity on the
|
||||
* physical height motor (axis 01). Preserve the real seat position
|
||||
* across OTA by reflecting its Hall coordinate into the corrected
|
||||
* version-2 coordinate system. The next state save commits v2. */
|
||||
uint32_t legacyRange =
|
||||
record.range[OTA_SEAT_STATE_REVERSED_DIRECTION_AXIS];
|
||||
uint32_t legacyPosition =
|
||||
record.position[OTA_SEAT_STATE_REVERSED_DIRECTION_AXIS];
|
||||
uint32_t corrected = legacyRange - legacyPosition;
|
||||
state->position[OTA_SEAT_STATE_REVERSED_DIRECTION_AXIS] =
|
||||
(corrected <= OTA_SEAT_AXIS1_FIXED_HALL_RANGE) ?
|
||||
corrected : OTA_SEAT_AXIS1_FIXED_HALL_RANGE;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool OtaSeatState_Save(const ota_seat_state_data_t *state)
|
||||
{
|
||||
ota_seat_state_record_t record;
|
||||
const uint32_t *words = (const uint32_t *)&record;
|
||||
const uint32_t wordCount =
|
||||
(uint32_t)(sizeof(record) / sizeof(uint32_t));
|
||||
const volatile uint32_t *stored =
|
||||
(const volatile uint32_t *)OTA_SEAT_STATE_ADDRESS;
|
||||
uint32_t axis;
|
||||
uint32_t index;
|
||||
bool success = true;
|
||||
|
||||
if((NULL == state) || ((state->validMask & ~0x0Ful) != 0ul))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
record.magic = OTA_SEAT_STATE_MAGIC;
|
||||
record.formatVersion = OTA_SEAT_STATE_VERSION;
|
||||
record.sequence = state->sequence;
|
||||
record.validMask = state->validMask;
|
||||
for(axis = 0ul; axis < OTA_SEAT_AXIS_COUNT; axis++)
|
||||
{
|
||||
/* Applications own position only. The immutable Base owns range. */
|
||||
record.range[axis] = otaSeatFixedHallRange[axis];
|
||||
record.position[axis] = state->position[axis];
|
||||
}
|
||||
for(axis = 0ul; axis < OTA_SEAT_AXIS_COUNT; axis++)
|
||||
{
|
||||
if(((state->validMask & (1ul << axis)) != 0ul) &&
|
||||
(state->position[axis] > otaSeatFixedHallRange[axis]))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
record.crc32 = OtaSeatState_Crc32(
|
||||
(const uint8_t *)&record,
|
||||
(uint32_t)offsetof(ota_seat_state_record_t, crc32));
|
||||
|
||||
Cy_Flashc_WorkWriteEnable();
|
||||
success = OtaSeatState_EraseSector();
|
||||
for(index = 0ul; (index < wordCount) && (false != success); index++)
|
||||
{
|
||||
success = OtaSeatState_ProgramWord(
|
||||
OTA_SEAT_STATE_ADDRESS + index * sizeof(uint32_t), &words[index]);
|
||||
}
|
||||
Cy_Flashc_WorkWriteDisable();
|
||||
Cy_Flashc_InvalidateFlashCacheBuffer();
|
||||
if(false != success)
|
||||
{
|
||||
for(index = 0ul; index < wordCount; index++)
|
||||
{
|
||||
if(stored[index] != words[index])
|
||||
{
|
||||
success = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
return success;
|
||||
}
|
||||
@@ -0,0 +1,26 @@
|
||||
#ifndef OTA_SEAT_STATE_H
|
||||
#define OTA_SEAT_STATE_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#define OTA_SEAT_AXIS_COUNT (4u)
|
||||
|
||||
#define OTA_SEAT_AXIS0_FIXED_HALL_RANGE (480ul)
|
||||
#define OTA_SEAT_AXIS1_FIXED_HALL_RANGE (373ul)
|
||||
#define OTA_SEAT_AXIS2_FIXED_HALL_RANGE (917ul)
|
||||
#define OTA_SEAT_AXIS3_FIXED_HALL_RANGE (647ul)
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint32_t sequence;
|
||||
uint32_t validMask;
|
||||
/* Read-only fixed Base values. OtaSeatState_Save() ignores caller changes. */
|
||||
uint32_t range[OTA_SEAT_AXIS_COUNT];
|
||||
uint32_t position[OTA_SEAT_AXIS_COUNT];
|
||||
} ota_seat_state_data_t;
|
||||
|
||||
bool OtaSeatState_Load(ota_seat_state_data_t *state);
|
||||
bool OtaSeatState_Save(const ota_seat_state_data_t *state);
|
||||
|
||||
#endif /* OTA_SEAT_STATE_H */
|
||||
@@ -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();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,18 @@
|
||||
#ifndef OTA_SELF_COMPOSE_PLAN_H
|
||||
#define OTA_SELF_COMPOSE_PLAN_H
|
||||
|
||||
/*
|
||||
* Canonical Codex-editable action plan. The GCC source generator copies this
|
||||
* file into a clean generated tree before every build. A zero step count
|
||||
* cannot start a motor, so the authoritative Base remains neutral.
|
||||
*/
|
||||
#define OTA_SELF_COMPOSE_STEP_COUNT (0u)
|
||||
#define OTA_SELF_COMPOSE_PROGRAM_REPEAT (1u)
|
||||
#define OTA_SELF_COMPOSE_MAX_RUNTIME_MS (0ul)
|
||||
|
||||
static const ota_self_compose_step_t otaSelfComposePlan[] =
|
||||
{
|
||||
OTA_SEAT_SLIDE_TO(0ul, OTA_SEAT_SPEED_DEFAULT, 0ul),
|
||||
};
|
||||
|
||||
#endif /* OTA_SELF_COMPOSE_PLAN_H */
|
||||
@@ -0,0 +1,28 @@
|
||||
#ifndef SEAT_DEMO_COMPAT_H
|
||||
#define SEAT_DEMO_COMPAT_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
/* The compatibility runtime shares the production CAN0 instance and the
|
||||
* production four-axis motor state. It never initializes another CAN block
|
||||
* and Demo motion deliberately has no heartbeat/lease dependency. */
|
||||
void SeatDemoCompat_Init(void);
|
||||
void SeatDemoCompat_Service(void);
|
||||
|
||||
/* Ownership hand-off used by the native 0x6A0 runtime. */
|
||||
void SeatDemoCompat_ReleaseAxisForNative(uint32_t axis);
|
||||
void SeatDemoCompat_StopAllFromNative(void);
|
||||
void SeatDemoCompat_PrepareForBootloader(void);
|
||||
bool SeatDemoCompat_IsAxisOwned(uint32_t axis);
|
||||
bool SeatDemoCompat_IsAnyMotorOwned(void);
|
||||
|
||||
/* Native 0x6A0 absolute moves share the same four-axis task engine as the
|
||||
* Demo 0x300 protocol. A zero duty selects the Demo default; 1..95 is exact. */
|
||||
uint8_t SeatDemoCompat_StartNativeMove(
|
||||
uint8_t sequence,
|
||||
uint32_t axis,
|
||||
uint32_t targetPermille,
|
||||
uint8_t dutyPercent);
|
||||
|
||||
#endif /* SEAT_DEMO_COMPAT_H */
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,997 @@
|
||||
/* Native Seat_Controller_demo rocking-horse and cradle runtime.
|
||||
*
|
||||
* The 0x300/0x0E command selects one of the original Demo modes. Targets,
|
||||
* PWM duties, rendezvous distances, phase timeouts and the 0x306 status frame
|
||||
* match Cognitive_Agent-feature-dynamic-mode. All four motors share the
|
||||
* production motor/CAN implementation; there is no heartbeat lease. */
|
||||
|
||||
#define SEAT_DEMO_DYNAMIC_STATUS_ID (0x306ul)
|
||||
#define SEAT_DEMO_DYNAMIC_STATUS_MS (100ul)
|
||||
#define SEAT_DEMO_DYNAMIC_MAX_RUNTIME_MS (600000ul)
|
||||
#define SEAT_DEMO_DYNAMIC_ENTRY_TIMEOUT_MS (30000ul)
|
||||
#define SEAT_DEMO_DYNAMIC_TARGET_TOLERANCE_HALL (3ul)
|
||||
#define SEAT_DEMO_DYNAMIC_ENTRY_DUTY (90u)
|
||||
#define SEAT_DEMO_DYNAMIC_FINAL_DUTY (50u)
|
||||
#define SEAT_DEMO_DYNAMIC_HORSE_TIMEOUT_MS (15000ul)
|
||||
#define SEAT_DEMO_DYNAMIC_CRADLE_TIMEOUT_MS (30000ul)
|
||||
#define SEAT_DEMO_DYNAMIC_HORSE_RENDEZVOUS (8ul)
|
||||
#define SEAT_DEMO_DYNAMIC_CRADLE_RENDEZVOUS (12ul)
|
||||
#define SEAT_DEMO_DYNAMIC_HORSE_SYNC_MOTOR_COUNT (2ul)
|
||||
#define SEAT_DEMO_DYNAMIC_HORSE_SYNC_MASK (0x03u)
|
||||
#define SEAT_DEMO_DYNAMIC_HORSE_START_DUTY (50u)
|
||||
#define SEAT_DEMO_DYNAMIC_HORSE_RAMP_MS (1000ul)
|
||||
#define SEAT_DEMO_DYNAMIC_HORSE_RAMP_STEP_MS (50ul)
|
||||
#define SEAT_DEMO_DYNAMIC_HORSE_DUTY_BOOST_PERCENT (5u)
|
||||
#define SEAT_DEMO_DYNAMIC_INVALID_AXIS (0xFFu)
|
||||
|
||||
typedef enum
|
||||
{
|
||||
SEAT_DEMO_DYNAMIC_MODE_NONE = 0u,
|
||||
SEAT_DEMO_DYNAMIC_MODE_ROCKING_HORSE = 1u,
|
||||
SEAT_DEMO_DYNAMIC_MODE_CRADLE = 2u
|
||||
} seat_demo_dynamic_mode_t;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
SEAT_DEMO_DYNAMIC_STATE_IDLE = 0u,
|
||||
SEAT_DEMO_DYNAMIC_STATE_ENTRY = 1u,
|
||||
SEAT_DEMO_DYNAMIC_STATE_RUNNING = 2u,
|
||||
SEAT_DEMO_DYNAMIC_STATE_FAULT = 3u
|
||||
} seat_demo_dynamic_state_t;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
SEAT_DEMO_DYNAMIC_PHASE_NONE = 0u,
|
||||
SEAT_DEMO_DYNAMIC_PHASE_ENTRY = 1u,
|
||||
SEAT_DEMO_DYNAMIC_PHASE_HORSE_FORWARD = 2u,
|
||||
SEAT_DEMO_DYNAMIC_PHASE_HORSE_BACKWARD = 3u,
|
||||
SEAT_DEMO_DYNAMIC_PHASE_CRADLE_FORWARD = 5u,
|
||||
SEAT_DEMO_DYNAMIC_PHASE_CRADLE_RETURN = 6u
|
||||
} seat_demo_dynamic_phase_t;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
SEAT_DEMO_DYNAMIC_HORSE_MOTION_NONE = 0u,
|
||||
SEAT_DEMO_DYNAMIC_HORSE_MOTION_RAMP_UP = 1u,
|
||||
SEAT_DEMO_DYNAMIC_HORSE_MOTION_CRUISE = 2u,
|
||||
SEAT_DEMO_DYNAMIC_HORSE_MOTION_RAMP_DOWN = 3u,
|
||||
SEAT_DEMO_DYNAMIC_HORSE_MOTION_SETTLING = 4u
|
||||
} seat_demo_dynamic_horse_motion_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint8_t mode;
|
||||
uint8_t state;
|
||||
uint8_t phase;
|
||||
uint8_t phaseIndex;
|
||||
uint8_t rendezvousStage;
|
||||
uint8_t faultMask;
|
||||
uint8_t pendingPhaseIndex;
|
||||
uint8_t horseMotion;
|
||||
uint8_t horseDirectionFarMask;
|
||||
uint8_t horseReachedMask;
|
||||
uint8_t horseBoostAxis[2];
|
||||
uint8_t horseCruiseDuty[SEAT_DEMO_MOTOR_COUNT];
|
||||
uint8_t horseAppliedDuty[SEAT_DEMO_MOTOR_COUNT];
|
||||
uint8_t horseDecelStartDuty[SEAT_DEMO_MOTOR_COUNT];
|
||||
uint16_t cycleIndex;
|
||||
uint16_t commandSequence;
|
||||
uint32_t modeStartedTicks;
|
||||
uint32_t phaseStartedTicks;
|
||||
uint32_t phaseTimeoutMs;
|
||||
uint32_t horseRampStartedTicks;
|
||||
uint32_t targetHall[SEAT_DEMO_MOTOR_COUNT];
|
||||
uint32_t finalTargetHall[SEAT_DEMO_MOTOR_COUNT];
|
||||
} seat_demo_dynamic_context_t;
|
||||
|
||||
/* M0/M1/M2/M3 = longitudinal, cushion height, backrest, legrest. */
|
||||
static const uint16_t seatDemoDynamicEntry[2][SEAT_DEMO_MOTOR_COUNT] =
|
||||
{
|
||||
{ 320u, 190u, 500u, 430u },
|
||||
{ 391u, 140u, 405u, 404u }
|
||||
};
|
||||
|
||||
static const uint16_t seatDemoDynamicTargets[2][2][SEAT_DEMO_MOTOR_COUNT] =
|
||||
{
|
||||
{
|
||||
{ 270u, 250u, 460u, 400u },
|
||||
{ 370u, 130u, 540u, 460u }
|
||||
},
|
||||
{
|
||||
{ 246u, 281u, 701u, 494u },
|
||||
{ 391u, 140u, 405u, 404u }
|
||||
}
|
||||
};
|
||||
|
||||
static const uint8_t seatDemoDynamicCruiseDuty[2][SEAT_DEMO_MOTOR_COUNT] =
|
||||
{
|
||||
{ 90u, 90u, 90u, 90u },
|
||||
{ 50u, 50u, 90u, 50u }
|
||||
};
|
||||
|
||||
static seat_demo_dynamic_context_t seatDemoDynamic;
|
||||
static bool seatDemoDynamicStatusPending;
|
||||
static bool seatDemoDynamicSavePending;
|
||||
static uint32_t seatDemoDynamicNextStatusTicks;
|
||||
|
||||
static bool SeatDemoDynamic_IsActive(void)
|
||||
{
|
||||
return (seatDemoDynamic.state == SEAT_DEMO_DYNAMIC_STATE_ENTRY) ||
|
||||
(seatDemoDynamic.state == SEAT_DEMO_DYNAMIC_STATE_RUNNING);
|
||||
}
|
||||
|
||||
static uint8_t SeatDemoDynamic_ActiveMask(void)
|
||||
{
|
||||
uint32_t axis;
|
||||
uint8_t mask = 0u;
|
||||
uint32_t interruptState;
|
||||
|
||||
if(false == SeatDemoDynamic_IsActive())
|
||||
{
|
||||
return 0u;
|
||||
}
|
||||
interruptState = Cy_SysLib_EnterCriticalSection();
|
||||
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
||||
{
|
||||
if((motorRuntime[axis].direction != MOTOR_DIRECTION_STOPPED) ||
|
||||
(false != motorRuntime[axis].startPending) ||
|
||||
(false != motorRuntime[axis].targetActive) ||
|
||||
(false != motorRuntime[axis].coastTracking))
|
||||
{
|
||||
mask |= (uint8_t)(1u << axis);
|
||||
}
|
||||
}
|
||||
Cy_SysLib_ExitCriticalSection(interruptState);
|
||||
return mask;
|
||||
}
|
||||
|
||||
static uint8_t SeatDemoDynamic_FaultMask(void)
|
||||
{
|
||||
return seatDemoDynamic.faultMask;
|
||||
}
|
||||
|
||||
static void SeatDemoDynamic_MarkFeedback(void)
|
||||
{
|
||||
seatDemoDynamicStatusPending = true;
|
||||
seatDemoSystemStatusPending = true;
|
||||
seatDemoMotorStatusPendingMask |= SEAT_DEMO_ACTIVE_MASK;
|
||||
seatDemoDiagnosticPendingMask |= SEAT_DEMO_ACTIVE_MASK;
|
||||
}
|
||||
|
||||
static void SeatDemoDynamic_StopMotors(void)
|
||||
{
|
||||
uint32_t axis;
|
||||
|
||||
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
||||
{
|
||||
(void)Motor_StopMode((motor_mode_t)axis);
|
||||
}
|
||||
}
|
||||
|
||||
static bool SeatDemoDynamic_HorseAllMotorsSettled(void)
|
||||
{
|
||||
uint32_t axis;
|
||||
uint32_t interruptState;
|
||||
bool settled = true;
|
||||
|
||||
interruptState = Cy_SysLib_EnterCriticalSection();
|
||||
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
||||
{
|
||||
if((motorRuntime[axis].direction != MOTOR_DIRECTION_STOPPED) ||
|
||||
(false != motorRuntime[axis].startPending) ||
|
||||
(false != motorRuntime[axis].targetActive) ||
|
||||
(false != motorRuntime[axis].coastTracking))
|
||||
{
|
||||
settled = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
Cy_SysLib_ExitCriticalSection(interruptState);
|
||||
return settled;
|
||||
}
|
||||
|
||||
static bool SeatDemoDynamic_AllMotorsSettled(void)
|
||||
{
|
||||
uint32_t axis;
|
||||
uint32_t interruptState;
|
||||
bool settled = true;
|
||||
|
||||
interruptState = Cy_SysLib_EnterCriticalSection();
|
||||
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
||||
{
|
||||
if((motorRuntime[axis].direction != MOTOR_DIRECTION_STOPPED) ||
|
||||
(false != motorRuntime[axis].startPending) ||
|
||||
(false != motorRuntime[axis].targetActive) ||
|
||||
(false != motorRuntime[axis].coastTracking))
|
||||
{
|
||||
settled = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
Cy_SysLib_ExitCriticalSection(interruptState);
|
||||
return settled;
|
||||
}
|
||||
|
||||
static bool SeatDemoDynamic_StartTargets(const uint32_t targets[],
|
||||
const uint8_t duties[],
|
||||
bool coastPastHorseTargets)
|
||||
{
|
||||
uint32_t axis;
|
||||
uint32_t targetValueX100[SEAT_DEMO_MOTOR_COUNT];
|
||||
motor_safety_options_t safety = { false, false };
|
||||
|
||||
if(false != coastPastHorseTargets)
|
||||
{
|
||||
seatDemoDynamic.horseDirectionFarMask = 0u;
|
||||
}
|
||||
|
||||
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
||||
{
|
||||
motor_position_info_t info;
|
||||
uint32_t commandedTargetHall = targets[axis];
|
||||
|
||||
if((MOTOR_API_SUCCESS != Motor_GetPositionInfo(
|
||||
(motor_mode_t)axis, &info)) ||
|
||||
(false == info.positionValid) ||
|
||||
(0ul == info.maxHallCount) ||
|
||||
(0ul == info.maxValueX100) ||
|
||||
(targets[axis] > info.maxHallCount))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if(false != coastPastHorseTargets)
|
||||
{
|
||||
uint32_t current;
|
||||
uint32_t interruptState = Cy_SysLib_EnterCriticalSection();
|
||||
|
||||
current = motorRuntime[axis].positionHallCount;
|
||||
Cy_SysLib_ExitCriticalSection(interruptState);
|
||||
if(targets[axis] > current)
|
||||
{
|
||||
commandedTargetHall = info.maxHallCount;
|
||||
seatDemoDynamic.horseDirectionFarMask |=
|
||||
(uint8_t)(1u << axis);
|
||||
}
|
||||
else if(targets[axis] < current)
|
||||
{
|
||||
commandedTargetHall = 0ul;
|
||||
}
|
||||
/* An exact target is left unchanged. The coupled M0/M1
|
||||
* arrival check will immediately start the smooth stop. */
|
||||
}
|
||||
targetValueX100[axis] = (uint32_t)
|
||||
(((uint64_t)commandedTargetHall * info.maxValueX100 +
|
||||
(info.maxHallCount / 2ul)) / info.maxHallCount);
|
||||
}
|
||||
|
||||
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
||||
{
|
||||
if((MOTOR_API_SUCCESS != Motor_SetSafetyOptions(
|
||||
(motor_mode_t)axis, &safety)) ||
|
||||
(MOTOR_API_SUCCESS != Motor_DisableSpeedControl(
|
||||
(motor_mode_t)axis)) ||
|
||||
(MOTOR_API_SUCCESS != Motor_SetDutyPercent(
|
||||
(motor_mode_t)axis, duties[axis])))
|
||||
{
|
||||
SeatDemoDynamic_StopMotors();
|
||||
return false;
|
||||
}
|
||||
}
|
||||
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
||||
{
|
||||
if(MOTOR_API_SUCCESS != Motor_StartMoveToValue(
|
||||
(motor_mode_t)axis, targetValueX100[axis]))
|
||||
{
|
||||
SeatDemoDynamic_StopMotors();
|
||||
return false;
|
||||
}
|
||||
otaSeatPersistedMask &= ~(1ul << axis);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool SeatDemoDynamic_TargetsReached(bool *failed)
|
||||
{
|
||||
uint32_t axis;
|
||||
bool reached = true;
|
||||
|
||||
*failed = false;
|
||||
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
||||
{
|
||||
uint32_t position;
|
||||
uint32_t difference;
|
||||
motor_event_t event;
|
||||
bool valid;
|
||||
bool moving;
|
||||
uint32_t interruptState = Cy_SysLib_EnterCriticalSection();
|
||||
|
||||
position = motorRuntime[axis].positionHallCount;
|
||||
event = motorRuntime[axis].lastEvent;
|
||||
valid = motorRuntime[axis].positionValid;
|
||||
moving = (motorRuntime[axis].direction != MOTOR_DIRECTION_STOPPED) ||
|
||||
(false != motorRuntime[axis].startPending) ||
|
||||
(false != motorRuntime[axis].targetActive) ||
|
||||
(false != motorRuntime[axis].coastTracking);
|
||||
Cy_SysLib_ExitCriticalSection(interruptState);
|
||||
|
||||
difference = (position >= seatDemoDynamic.targetHall[axis]) ?
|
||||
(position - seatDemoDynamic.targetHall[axis]) :
|
||||
(seatDemoDynamic.targetHall[axis] - position);
|
||||
if((false == valid) ||
|
||||
(event == MOTOR_EVENT_STALL) ||
|
||||
(event == MOTOR_EVENT_TIMEOUT) ||
|
||||
(event == MOTOR_EVENT_DRIVER_FAULT) ||
|
||||
(((event == MOTOR_EVENT_NEAR_ENDPOINT) ||
|
||||
(event == MOTOR_EVENT_FAR_ENDPOINT)) &&
|
||||
(difference > SEAT_DEMO_DYNAMIC_TARGET_TOLERANCE_HALL)))
|
||||
{
|
||||
seatDemoDynamic.faultMask |= (uint8_t)(1u << axis);
|
||||
*failed = true;
|
||||
}
|
||||
if((false != moving) ||
|
||||
(difference > SEAT_DEMO_DYNAMIC_TARGET_TOLERANCE_HALL))
|
||||
{
|
||||
reached = false;
|
||||
}
|
||||
}
|
||||
return reached;
|
||||
}
|
||||
|
||||
static uint8_t SeatDemoDynamic_ModeIndex(void)
|
||||
{
|
||||
return (seatDemoDynamic.mode ==
|
||||
SEAT_DEMO_DYNAMIC_MODE_ROCKING_HORSE) ? 0u : 1u;
|
||||
}
|
||||
|
||||
static uint32_t SeatDemoDynamic_PhaseTimeoutMs(void)
|
||||
{
|
||||
return (seatDemoDynamic.mode ==
|
||||
SEAT_DEMO_DYNAMIC_MODE_ROCKING_HORSE) ?
|
||||
SEAT_DEMO_DYNAMIC_HORSE_TIMEOUT_MS :
|
||||
SEAT_DEMO_DYNAMIC_CRADLE_TIMEOUT_MS;
|
||||
}
|
||||
|
||||
static uint32_t SeatDemoDynamic_RendezvousPulses(void)
|
||||
{
|
||||
return (seatDemoDynamic.mode ==
|
||||
SEAT_DEMO_DYNAMIC_MODE_ROCKING_HORSE) ?
|
||||
SEAT_DEMO_DYNAMIC_HORSE_RENDEZVOUS :
|
||||
SEAT_DEMO_DYNAMIC_CRADLE_RENDEZVOUS;
|
||||
}
|
||||
|
||||
static void SeatDemoDynamic_ResolveCruiseDuties(
|
||||
uint8_t modeIndex,
|
||||
uint8_t phaseIndex,
|
||||
uint8_t duties[])
|
||||
{
|
||||
uint32_t axis;
|
||||
|
||||
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
||||
{
|
||||
duties[axis] = seatDemoDynamicCruiseDuty[modeIndex][axis];
|
||||
}
|
||||
if((seatDemoDynamic.mode == SEAT_DEMO_DYNAMIC_MODE_ROCKING_HORSE) &&
|
||||
(phaseIndex < 2u) &&
|
||||
(seatDemoDynamic.horseBoostAxis[phaseIndex] <
|
||||
SEAT_DEMO_DYNAMIC_HORSE_SYNC_MOTOR_COUNT))
|
||||
{
|
||||
uint8_t boostAxis = seatDemoDynamic.horseBoostAxis[phaseIndex];
|
||||
uint32_t boosted = (uint32_t)
|
||||
duties[boostAxis] +
|
||||
SEAT_DEMO_DYNAMIC_HORSE_DUTY_BOOST_PERCENT;
|
||||
|
||||
if(boosted > MOTOR_FIXED_DUTY_MAX_PERCENT)
|
||||
{
|
||||
boosted = MOTOR_FIXED_DUTY_MAX_PERCENT;
|
||||
}
|
||||
duties[boostAxis] = (uint8_t)boosted;
|
||||
}
|
||||
}
|
||||
|
||||
static bool SeatDemoDynamic_StartEntry(uint32_t now)
|
||||
{
|
||||
uint32_t axis;
|
||||
uint32_t targets[SEAT_DEMO_MOTOR_COUNT];
|
||||
uint8_t duties[SEAT_DEMO_MOTOR_COUNT];
|
||||
uint8_t modeIndex = SeatDemoDynamic_ModeIndex();
|
||||
|
||||
seatDemoDynamic.state = SEAT_DEMO_DYNAMIC_STATE_ENTRY;
|
||||
seatDemoDynamic.phase = SEAT_DEMO_DYNAMIC_PHASE_ENTRY;
|
||||
seatDemoDynamic.phaseStartedTicks = now;
|
||||
seatDemoDynamic.phaseTimeoutMs = SEAT_DEMO_DYNAMIC_ENTRY_TIMEOUT_MS;
|
||||
seatDemoDynamic.rendezvousStage = 0u;
|
||||
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
||||
{
|
||||
targets[axis] = seatDemoDynamicEntry[modeIndex][axis];
|
||||
duties[axis] = SEAT_DEMO_DYNAMIC_ENTRY_DUTY;
|
||||
seatDemoDynamic.targetHall[axis] = targets[axis];
|
||||
seatDemoDynamic.finalTargetHall[axis] = targets[axis];
|
||||
}
|
||||
SeatDemoDynamic_MarkFeedback();
|
||||
return SeatDemoDynamic_StartTargets(targets, duties, false);
|
||||
}
|
||||
|
||||
static bool SeatDemoDynamic_StartPhase(uint8_t phaseIndex, uint32_t now)
|
||||
{
|
||||
uint32_t axis;
|
||||
uint32_t targets[SEAT_DEMO_MOTOR_COUNT];
|
||||
uint32_t rendezvous = SeatDemoDynamic_RendezvousPulses();
|
||||
uint8_t modeIndex = SeatDemoDynamic_ModeIndex();
|
||||
uint8_t duties[SEAT_DEMO_MOTOR_COUNT];
|
||||
bool rockingHorse =
|
||||
(seatDemoDynamic.mode == SEAT_DEMO_DYNAMIC_MODE_ROCKING_HORSE);
|
||||
bool firstFormalPhase =
|
||||
(seatDemoDynamic.state == SEAT_DEMO_DYNAMIC_STATE_ENTRY);
|
||||
|
||||
seatDemoDynamic.state = SEAT_DEMO_DYNAMIC_STATE_RUNNING;
|
||||
seatDemoDynamic.phaseIndex = phaseIndex;
|
||||
seatDemoDynamic.phase = (false != rockingHorse) ?
|
||||
(uint8_t)(SEAT_DEMO_DYNAMIC_PHASE_HORSE_FORWARD + phaseIndex) :
|
||||
(uint8_t)(SEAT_DEMO_DYNAMIC_PHASE_CRADLE_FORWARD + phaseIndex);
|
||||
seatDemoDynamic.phaseStartedTicks = now;
|
||||
seatDemoDynamic.phaseTimeoutMs = SeatDemoDynamic_PhaseTimeoutMs();
|
||||
seatDemoDynamic.rendezvousStage = (false != rockingHorse) ? 1u : 0u;
|
||||
seatDemoDynamic.pendingPhaseIndex = phaseIndex;
|
||||
seatDemoDynamic.horseReachedMask = 0u;
|
||||
if((false != firstFormalPhase) &&
|
||||
(false != rockingHorse))
|
||||
{
|
||||
/* Entry positioning has its own timeout and is deliberately excluded
|
||||
* from the requested rocking-cycle runtime. */
|
||||
seatDemoDynamic.modeStartedTicks = now;
|
||||
}
|
||||
|
||||
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
||||
{
|
||||
uint32_t current;
|
||||
uint32_t finalTarget =
|
||||
seatDemoDynamicTargets[modeIndex][phaseIndex][axis];
|
||||
uint32_t interruptState = Cy_SysLib_EnterCriticalSection();
|
||||
|
||||
current = motorRuntime[axis].positionHallCount;
|
||||
Cy_SysLib_ExitCriticalSection(interruptState);
|
||||
seatDemoDynamic.finalTargetHall[axis] = finalTarget;
|
||||
if(false != rockingHorse)
|
||||
{
|
||||
/* The motor core receives the physical endpoint as a guard target
|
||||
* so crossing this rocking target does not kill PWM before the
|
||||
* coordinated one-second ramp-down has completed. */
|
||||
targets[axis] = finalTarget;
|
||||
}
|
||||
else if(finalTarget > (current + rendezvous))
|
||||
{
|
||||
targets[axis] = finalTarget - rendezvous;
|
||||
}
|
||||
else if(current > (finalTarget + rendezvous))
|
||||
{
|
||||
targets[axis] = finalTarget + rendezvous;
|
||||
}
|
||||
else
|
||||
{
|
||||
targets[axis] = finalTarget;
|
||||
}
|
||||
seatDemoDynamic.targetHall[axis] = targets[axis];
|
||||
}
|
||||
SeatDemoDynamic_ResolveCruiseDuties(modeIndex, phaseIndex, duties);
|
||||
if(false != rockingHorse)
|
||||
{
|
||||
seatDemoDynamic.horseMotion =
|
||||
SEAT_DEMO_DYNAMIC_HORSE_MOTION_RAMP_UP;
|
||||
seatDemoDynamic.horseRampStartedTicks = now;
|
||||
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
||||
{
|
||||
seatDemoDynamic.horseCruiseDuty[axis] = duties[axis];
|
||||
seatDemoDynamic.horseAppliedDuty[axis] =
|
||||
SEAT_DEMO_DYNAMIC_HORSE_START_DUTY;
|
||||
seatDemoDynamic.horseDecelStartDuty[axis] =
|
||||
SEAT_DEMO_DYNAMIC_HORSE_START_DUTY;
|
||||
duties[axis] = SEAT_DEMO_DYNAMIC_HORSE_START_DUTY;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
seatDemoDynamic.horseMotion =
|
||||
SEAT_DEMO_DYNAMIC_HORSE_MOTION_NONE;
|
||||
}
|
||||
SeatDemoDynamic_MarkFeedback();
|
||||
return SeatDemoDynamic_StartTargets(targets, duties, rockingHorse);
|
||||
}
|
||||
|
||||
static uint8_t SeatDemoDynamic_HorseReachedMask(void)
|
||||
{
|
||||
uint32_t axis;
|
||||
uint8_t reachedMask = 0u;
|
||||
|
||||
for(axis = 0ul;
|
||||
axis < SEAT_DEMO_DYNAMIC_HORSE_SYNC_MOTOR_COUNT;
|
||||
axis++)
|
||||
{
|
||||
uint32_t position;
|
||||
uint32_t difference;
|
||||
uint32_t interruptState = Cy_SysLib_EnterCriticalSection();
|
||||
|
||||
position = motorRuntime[axis].positionHallCount;
|
||||
Cy_SysLib_ExitCriticalSection(interruptState);
|
||||
difference = (position >= seatDemoDynamic.targetHall[axis]) ?
|
||||
(position - seatDemoDynamic.targetHall[axis]) :
|
||||
(seatDemoDynamic.targetHall[axis] - position);
|
||||
if(difference <= SEAT_DEMO_DYNAMIC_TARGET_TOLERANCE_HALL)
|
||||
{
|
||||
reachedMask |= (uint8_t)(1u << axis);
|
||||
}
|
||||
else if(((seatDemoDynamic.horseDirectionFarMask &
|
||||
(uint8_t)(1u << axis)) != 0u) &&
|
||||
(position > seatDemoDynamic.targetHall[axis]))
|
||||
{
|
||||
reachedMask |= (uint8_t)(1u << axis);
|
||||
}
|
||||
else if(((seatDemoDynamic.horseDirectionFarMask &
|
||||
(uint8_t)(1u << axis)) == 0u) &&
|
||||
(position < seatDemoDynamic.targetHall[axis]))
|
||||
{
|
||||
reachedMask |= (uint8_t)(1u << axis);
|
||||
}
|
||||
}
|
||||
return reachedMask;
|
||||
}
|
||||
|
||||
static bool SeatDemoDynamic_HorseHasFault(void)
|
||||
{
|
||||
uint32_t axis;
|
||||
bool failed = false;
|
||||
|
||||
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
||||
{
|
||||
uint32_t position;
|
||||
uint32_t difference;
|
||||
motor_event_t event;
|
||||
bool valid;
|
||||
uint32_t interruptState = Cy_SysLib_EnterCriticalSection();
|
||||
|
||||
position = motorRuntime[axis].positionHallCount;
|
||||
event = motorRuntime[axis].lastEvent;
|
||||
valid = motorRuntime[axis].positionValid;
|
||||
Cy_SysLib_ExitCriticalSection(interruptState);
|
||||
difference = (position >= seatDemoDynamic.targetHall[axis]) ?
|
||||
(position - seatDemoDynamic.targetHall[axis]) :
|
||||
(seatDemoDynamic.targetHall[axis] - position);
|
||||
if((false == valid) ||
|
||||
(event == MOTOR_EVENT_STALL) ||
|
||||
(event == MOTOR_EVENT_TIMEOUT) ||
|
||||
(event == MOTOR_EVENT_DRIVER_FAULT) ||
|
||||
(((event == MOTOR_EVENT_TARGET_REACHED) ||
|
||||
(event == MOTOR_EVENT_NEAR_ENDPOINT) ||
|
||||
(event == MOTOR_EVENT_FAR_ENDPOINT)) &&
|
||||
(difference > SEAT_DEMO_DYNAMIC_TARGET_TOLERANCE_HALL)))
|
||||
{
|
||||
seatDemoDynamic.faultMask |= (uint8_t)(1u << axis);
|
||||
failed = true;
|
||||
}
|
||||
}
|
||||
return failed;
|
||||
}
|
||||
|
||||
static bool SeatDemoDynamic_HorseApplyDuties(const uint8_t duties[])
|
||||
{
|
||||
uint32_t axis;
|
||||
|
||||
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
||||
{
|
||||
if(duties[axis] == seatDemoDynamic.horseAppliedDuty[axis])
|
||||
{
|
||||
continue;
|
||||
}
|
||||
if(MOTOR_API_SUCCESS != Motor_SetDutyPercent(
|
||||
(motor_mode_t)axis, duties[axis]))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
seatDemoDynamic.horseAppliedDuty[axis] = duties[axis];
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static uint32_t SeatDemoDynamic_HorseRampElapsedMs(uint32_t now)
|
||||
{
|
||||
uint32_t elapsedMs = MOTOR_TICKS_TO_MS(
|
||||
now - seatDemoDynamic.horseRampStartedTicks);
|
||||
|
||||
if(elapsedMs >= SEAT_DEMO_DYNAMIC_HORSE_RAMP_MS)
|
||||
{
|
||||
return SEAT_DEMO_DYNAMIC_HORSE_RAMP_MS;
|
||||
}
|
||||
return (elapsedMs / SEAT_DEMO_DYNAMIC_HORSE_RAMP_STEP_MS) *
|
||||
SEAT_DEMO_DYNAMIC_HORSE_RAMP_STEP_MS;
|
||||
}
|
||||
|
||||
static bool SeatDemoDynamic_HorseRampUp(uint32_t now)
|
||||
{
|
||||
uint32_t axis;
|
||||
uint32_t elapsedMs = SeatDemoDynamic_HorseRampElapsedMs(now);
|
||||
uint8_t duties[SEAT_DEMO_MOTOR_COUNT];
|
||||
|
||||
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
||||
{
|
||||
uint32_t span = (uint32_t)
|
||||
seatDemoDynamic.horseCruiseDuty[axis] -
|
||||
SEAT_DEMO_DYNAMIC_HORSE_START_DUTY;
|
||||
uint32_t duty = SEAT_DEMO_DYNAMIC_HORSE_START_DUTY +
|
||||
((span * elapsedMs +
|
||||
(SEAT_DEMO_DYNAMIC_HORSE_RAMP_MS / 2ul)) /
|
||||
SEAT_DEMO_DYNAMIC_HORSE_RAMP_MS);
|
||||
|
||||
duties[axis] = (uint8_t)duty;
|
||||
}
|
||||
if(false == SeatDemoDynamic_HorseApplyDuties(duties))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if(elapsedMs >= SEAT_DEMO_DYNAMIC_HORSE_RAMP_MS)
|
||||
{
|
||||
seatDemoDynamic.horseMotion =
|
||||
SEAT_DEMO_DYNAMIC_HORSE_MOTION_CRUISE;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static void SeatDemoDynamic_HorseBeginRampDown(uint32_t now,
|
||||
uint8_t reachedMask)
|
||||
{
|
||||
uint32_t axis;
|
||||
uint8_t nextPhase = (uint8_t)(seatDemoDynamic.phaseIndex + 1u);
|
||||
|
||||
if(nextPhase >= 2u)
|
||||
{
|
||||
nextPhase = 0u;
|
||||
seatDemoDynamic.cycleIndex++;
|
||||
}
|
||||
seatDemoDynamic.pendingPhaseIndex = nextPhase;
|
||||
seatDemoDynamic.horseReachedMask = reachedMask;
|
||||
seatDemoDynamic.horseRampStartedTicks = now;
|
||||
seatDemoDynamic.horseMotion =
|
||||
SEAT_DEMO_DYNAMIC_HORSE_MOTION_RAMP_DOWN;
|
||||
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
||||
{
|
||||
seatDemoDynamic.horseDecelStartDuty[axis] =
|
||||
seatDemoDynamic.horseAppliedDuty[axis];
|
||||
}
|
||||
SeatDemoDynamic_MarkFeedback();
|
||||
}
|
||||
|
||||
static bool SeatDemoDynamic_HorseRampDown(uint32_t now)
|
||||
{
|
||||
uint32_t axis;
|
||||
uint32_t elapsedMs = SeatDemoDynamic_HorseRampElapsedMs(now);
|
||||
uint8_t duties[SEAT_DEMO_MOTOR_COUNT];
|
||||
|
||||
seatDemoDynamic.horseReachedMask |=
|
||||
SeatDemoDynamic_HorseReachedMask();
|
||||
if(elapsedMs >= SEAT_DEMO_DYNAMIC_HORSE_RAMP_MS)
|
||||
{
|
||||
uint8_t missingMask = (uint8_t)
|
||||
(SEAT_DEMO_DYNAMIC_HORSE_SYNC_MASK &
|
||||
(uint8_t)(~seatDemoDynamic.horseReachedMask));
|
||||
|
||||
if((missingMask != 0u) &&
|
||||
(seatDemoDynamic.horseBoostAxis[seatDemoDynamic.phaseIndex] ==
|
||||
SEAT_DEMO_DYNAMIC_INVALID_AXIS))
|
||||
{
|
||||
seatDemoDynamic.horseBoostAxis[seatDemoDynamic.phaseIndex] =
|
||||
((missingMask & 0x01u) != 0u) ? 0u : 1u;
|
||||
}
|
||||
/* Motor_SetDutyPercent deliberately rejects zero. Once the nonlinear
|
||||
* duty curve reaches the one-second boundary, stopping the timer is
|
||||
* the exact zero-percent step for all four bridges. */
|
||||
SeatDemoDynamic_StopMotors();
|
||||
seatDemoDynamic.horseMotion =
|
||||
SEAT_DEMO_DYNAMIC_HORSE_MOTION_SETTLING;
|
||||
SeatDemoDynamic_MarkFeedback();
|
||||
return true;
|
||||
}
|
||||
|
||||
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
||||
{
|
||||
uint64_t durationSquared =
|
||||
((uint64_t)SEAT_DEMO_DYNAMIC_HORSE_RAMP_MS) *
|
||||
SEAT_DEMO_DYNAMIC_HORSE_RAMP_MS;
|
||||
uint64_t elapsedSquared = ((uint64_t)elapsedMs) * elapsedMs;
|
||||
uint64_t remaining = durationSquared - elapsedSquared;
|
||||
uint32_t duty = (uint32_t)
|
||||
((((uint64_t)seatDemoDynamic.horseDecelStartDuty[axis]) *
|
||||
remaining + durationSquared - 1ull) / durationSquared);
|
||||
|
||||
if(duty < MOTOR_FIXED_DUTY_MIN_PERCENT)
|
||||
{
|
||||
duty = MOTOR_FIXED_DUTY_MIN_PERCENT;
|
||||
}
|
||||
duties[axis] = (uint8_t)duty;
|
||||
}
|
||||
return SeatDemoDynamic_HorseApplyDuties(duties);
|
||||
}
|
||||
|
||||
static bool SeatDemoDynamic_StartFinalApproach(void)
|
||||
{
|
||||
uint32_t axis;
|
||||
uint32_t targets[SEAT_DEMO_MOTOR_COUNT];
|
||||
uint8_t duties[SEAT_DEMO_MOTOR_COUNT];
|
||||
|
||||
seatDemoDynamic.rendezvousStage = 1u;
|
||||
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
||||
{
|
||||
targets[axis] = seatDemoDynamic.finalTargetHall[axis];
|
||||
seatDemoDynamic.targetHall[axis] = targets[axis];
|
||||
duties[axis] = SEAT_DEMO_DYNAMIC_FINAL_DUTY;
|
||||
}
|
||||
SeatDemoDynamic_MarkFeedback();
|
||||
return SeatDemoDynamic_StartTargets(targets, duties, false);
|
||||
}
|
||||
|
||||
static void SeatDemoDynamic_Stop(uint16_t sequence)
|
||||
{
|
||||
bool owned = (seatDemoDynamic.mode != SEAT_DEMO_DYNAMIC_MODE_NONE) ||
|
||||
(seatDemoDynamic.state != SEAT_DEMO_DYNAMIC_STATE_IDLE);
|
||||
|
||||
SeatDemoDynamic_StopMotors();
|
||||
(void)memset(&seatDemoDynamic, 0, sizeof(seatDemoDynamic));
|
||||
seatDemoDynamic.commandSequence = sequence;
|
||||
seatDemoDynamic.state = SEAT_DEMO_DYNAMIC_STATE_IDLE;
|
||||
seatDemoDynamicSavePending = true;
|
||||
if(false != owned)
|
||||
{
|
||||
otaSeatJobActive = false;
|
||||
otaSeatJobComplete = true;
|
||||
otaSeatJobSuccess = true;
|
||||
otaSeatJobType = OTA_SEAT_JOB_DYNAMIC;
|
||||
otaSeatJobPhase = OTA_SEAT_PHASE_STOPPED;
|
||||
otaSeatJobAxis = 0x0Fu;
|
||||
otaSeatJobError = 0u;
|
||||
otaSeatJobProgress = 100u;
|
||||
}
|
||||
SeatDemoDynamic_MarkFeedback();
|
||||
}
|
||||
|
||||
static void SeatDemoDynamic_Fail(void)
|
||||
{
|
||||
if(seatDemoDynamic.faultMask == 0u)
|
||||
{
|
||||
seatDemoDynamic.faultMask = SEAT_DEMO_ACTIVE_MASK;
|
||||
}
|
||||
SeatDemoDynamic_StopMotors();
|
||||
seatDemoDynamic.state = SEAT_DEMO_DYNAMIC_STATE_FAULT;
|
||||
seatDemoDynamicSavePending = true;
|
||||
otaSeatJobActive = false;
|
||||
otaSeatJobComplete = true;
|
||||
otaSeatJobSuccess = false;
|
||||
otaSeatJobType = OTA_SEAT_JOB_DYNAMIC;
|
||||
otaSeatJobPhase = OTA_SEAT_PHASE_STOPPED;
|
||||
otaSeatJobAxis = 0x0Fu;
|
||||
otaSeatJobError = 1u;
|
||||
SeatDemoDynamic_MarkFeedback();
|
||||
}
|
||||
|
||||
static bool SeatDemoDynamic_Start(uint8_t mode, uint16_t sequence)
|
||||
{
|
||||
uint32_t axis;
|
||||
|
||||
if((mode != SEAT_DEMO_DYNAMIC_MODE_ROCKING_HORSE) &&
|
||||
(mode != SEAT_DEMO_DYNAMIC_MODE_CRADLE))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
||||
{
|
||||
if(false == SeatDemoCompat_PositionValid(axis))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
(void)memset(&seatDemoDynamic, 0, sizeof(seatDemoDynamic));
|
||||
seatDemoDynamic.mode = mode;
|
||||
seatDemoDynamic.horseBoostAxis[0] = SEAT_DEMO_DYNAMIC_INVALID_AXIS;
|
||||
seatDemoDynamic.horseBoostAxis[1] = SEAT_DEMO_DYNAMIC_INVALID_AXIS;
|
||||
seatDemoDynamic.horseMotion =
|
||||
SEAT_DEMO_DYNAMIC_HORSE_MOTION_NONE;
|
||||
seatDemoDynamic.commandSequence = sequence;
|
||||
seatDemoDynamic.modeStartedTicks = hallTimebaseTicks;
|
||||
seatDemoDynamicSavePending = false;
|
||||
otaSeatJobActive = true;
|
||||
otaSeatJobComplete = false;
|
||||
otaSeatJobSuccess = false;
|
||||
otaSeatJobType = OTA_SEAT_JOB_DYNAMIC;
|
||||
otaSeatJobPhase = OTA_SEAT_PHASE_DYNAMIC;
|
||||
otaSeatJobAxis = 0x0Fu;
|
||||
otaSeatJobError = 0u;
|
||||
otaSeatJobProgress = 0u;
|
||||
OtaCan_ClearLatchedFaults();
|
||||
Cy_GPIO_Clr(DRV_DRVOFF_PORT, DRV_DRVOFF_PIN);
|
||||
Cy_SysTick_DelayInUs(100ul);
|
||||
if(false == SeatDemoDynamic_StartEntry(hallTimebaseTicks))
|
||||
{
|
||||
SeatDemoDynamic_Fail();
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static void SeatDemoDynamic_Service(void)
|
||||
{
|
||||
uint32_t now = hallTimebaseTicks;
|
||||
bool failed;
|
||||
bool reached;
|
||||
|
||||
if(false == SeatDemoDynamic_IsActive())
|
||||
{
|
||||
if((false != seatDemoDynamicSavePending) &&
|
||||
(false != SeatDemoDynamic_AllMotorsSettled()))
|
||||
{
|
||||
(void)OtaSeat_SavePersistentState();
|
||||
seatDemoDynamicSavePending = false;
|
||||
}
|
||||
return;
|
||||
}
|
||||
if(false != driverFaultLatched)
|
||||
{
|
||||
seatDemoDynamic.faultMask = SEAT_DEMO_ACTIVE_MASK;
|
||||
SeatDemoDynamic_Fail();
|
||||
return;
|
||||
}
|
||||
if((seatDemoDynamic.state == SEAT_DEMO_DYNAMIC_STATE_ENTRY) &&
|
||||
(seatDemoDynamic.mode == SEAT_DEMO_DYNAMIC_MODE_ROCKING_HORSE))
|
||||
{
|
||||
otaSeatJobProgress = 0u;
|
||||
}
|
||||
else
|
||||
{
|
||||
otaSeatJobProgress = (uint8_t)
|
||||
((MOTOR_TICKS_TO_MS(now - seatDemoDynamic.modeStartedTicks) *
|
||||
100ul) / SEAT_DEMO_DYNAMIC_MAX_RUNTIME_MS);
|
||||
if(false != SeatDemoCompat_TimeReached(
|
||||
now, seatDemoDynamic.modeStartedTicks +
|
||||
MOTOR_MS_TO_TICKS(SEAT_DEMO_DYNAMIC_MAX_RUNTIME_MS)))
|
||||
{
|
||||
SeatDemoDynamic_Stop((uint16_t)
|
||||
(seatDemoDynamic.commandSequence + 1u));
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
if((seatDemoDynamic.mode == SEAT_DEMO_DYNAMIC_MODE_ROCKING_HORSE) &&
|
||||
(seatDemoDynamic.state == SEAT_DEMO_DYNAMIC_STATE_RUNNING))
|
||||
{
|
||||
uint8_t reachedMask;
|
||||
|
||||
if(false != SeatDemoDynamic_HorseHasFault())
|
||||
{
|
||||
SeatDemoDynamic_Fail();
|
||||
return;
|
||||
}
|
||||
if(seatDemoDynamic.horseMotion ==
|
||||
SEAT_DEMO_DYNAMIC_HORSE_MOTION_SETTLING)
|
||||
{
|
||||
if(false != SeatDemoDynamic_HorseAllMotorsSettled())
|
||||
{
|
||||
if(false == SeatDemoDynamic_StartPhase(
|
||||
seatDemoDynamic.pendingPhaseIndex, now))
|
||||
{
|
||||
SeatDemoDynamic_Fail();
|
||||
}
|
||||
}
|
||||
else if(false != SeatDemoCompat_TimeReached(
|
||||
now, seatDemoDynamic.phaseStartedTicks +
|
||||
MOTOR_MS_TO_TICKS(
|
||||
seatDemoDynamic.phaseTimeoutMs)))
|
||||
{
|
||||
SeatDemoDynamic_Fail();
|
||||
}
|
||||
return;
|
||||
}
|
||||
if(seatDemoDynamic.horseMotion ==
|
||||
SEAT_DEMO_DYNAMIC_HORSE_MOTION_RAMP_DOWN)
|
||||
{
|
||||
if(false == SeatDemoDynamic_HorseRampDown(now))
|
||||
{
|
||||
SeatDemoDynamic_Fail();
|
||||
}
|
||||
return;
|
||||
}
|
||||
if(seatDemoDynamic.horseMotion ==
|
||||
SEAT_DEMO_DYNAMIC_HORSE_MOTION_RAMP_UP)
|
||||
{
|
||||
if(false == SeatDemoDynamic_HorseRampUp(now))
|
||||
{
|
||||
SeatDemoDynamic_Fail();
|
||||
return;
|
||||
}
|
||||
}
|
||||
reachedMask = SeatDemoDynamic_HorseReachedMask();
|
||||
if((reachedMask & SEAT_DEMO_DYNAMIC_HORSE_SYNC_MASK) != 0u)
|
||||
{
|
||||
SeatDemoDynamic_HorseBeginRampDown(now, reachedMask);
|
||||
return;
|
||||
}
|
||||
if(false != SeatDemoCompat_TimeReached(
|
||||
now, seatDemoDynamic.phaseStartedTicks +
|
||||
MOTOR_MS_TO_TICKS(seatDemoDynamic.phaseTimeoutMs)))
|
||||
{
|
||||
SeatDemoDynamic_Fail();
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
reached = SeatDemoDynamic_TargetsReached(&failed);
|
||||
if(false != failed)
|
||||
{
|
||||
SeatDemoDynamic_Fail();
|
||||
return;
|
||||
}
|
||||
if(false != reached)
|
||||
{
|
||||
if(seatDemoDynamic.state == SEAT_DEMO_DYNAMIC_STATE_ENTRY)
|
||||
{
|
||||
if(false == SeatDemoDynamic_StartPhase(0u, now))
|
||||
{
|
||||
SeatDemoDynamic_Fail();
|
||||
}
|
||||
}
|
||||
else if(seatDemoDynamic.rendezvousStage == 0u)
|
||||
{
|
||||
if(false == SeatDemoDynamic_StartFinalApproach())
|
||||
{
|
||||
SeatDemoDynamic_Fail();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
uint8_t nextPhase = (uint8_t)(seatDemoDynamic.phaseIndex + 1u);
|
||||
if(nextPhase >= 2u)
|
||||
{
|
||||
nextPhase = 0u;
|
||||
seatDemoDynamic.cycleIndex++;
|
||||
}
|
||||
if(false == SeatDemoDynamic_StartPhase(nextPhase, now))
|
||||
{
|
||||
SeatDemoDynamic_Fail();
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
if(false != SeatDemoCompat_TimeReached(
|
||||
now, seatDemoDynamic.phaseStartedTicks +
|
||||
MOTOR_MS_TO_TICKS(seatDemoDynamic.phaseTimeoutMs)))
|
||||
{
|
||||
SeatDemoDynamic_Fail();
|
||||
}
|
||||
}
|
||||
|
||||
static bool SeatDemoDynamic_SendStatus(void)
|
||||
{
|
||||
uint8_t data[8];
|
||||
uint32_t elapsedMs = 0ul;
|
||||
|
||||
if(false != SeatDemoDynamic_IsActive())
|
||||
{
|
||||
elapsedMs = MOTOR_TICKS_TO_MS(
|
||||
hallTimebaseTicks - seatDemoDynamic.phaseStartedTicks);
|
||||
if(elapsedMs > 0xFFFFul)
|
||||
{
|
||||
elapsedMs = 0xFFFFul;
|
||||
}
|
||||
}
|
||||
data[0] = seatDemoDynamic.mode;
|
||||
data[1] = seatDemoDynamic.state;
|
||||
data[2] = seatDemoDynamic.phase;
|
||||
data[3] = (uint8_t)((SeatDemoDynamic_ActiveMask() & 0x0Fu) |
|
||||
((seatDemoDynamic.faultMask & 0x0Fu) << 4u));
|
||||
SeatDemoCompat_WriteLe16(&data[4], seatDemoDynamic.cycleIndex);
|
||||
SeatDemoCompat_WriteLe16(&data[6], (uint16_t)elapsedMs);
|
||||
return OtaCan_QueueFrame(SEAT_DEMO_DYNAMIC_STATUS_ID, data);
|
||||
}
|
||||
|
||||
static void SeatDemoDynamic_Init(void)
|
||||
{
|
||||
(void)memset(&seatDemoDynamic, 0, sizeof(seatDemoDynamic));
|
||||
seatDemoDynamic.state = SEAT_DEMO_DYNAMIC_STATE_IDLE;
|
||||
seatDemoDynamicStatusPending = true;
|
||||
seatDemoDynamicSavePending = false;
|
||||
seatDemoDynamicNextStatusTicks = hallTimebaseTicks;
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,16 @@
|
||||
#ifndef SEAT_LEGACY_VERSION_QUERY_H
|
||||
#define SEAT_LEGACY_VERSION_QUERY_H
|
||||
|
||||
/* ota_can_frame_t is supplied by ota_can.h before the compatibility runtime. */
|
||||
static inline bool SeatDemo_IsLegacyVersionQuery(const ota_can_frame_t *frame)
|
||||
{
|
||||
return (NULL != frame) &&
|
||||
(frame->identifier == 0x300ul) &&
|
||||
(frame->dlc == 8u) &&
|
||||
(frame->data[0] == 0x10u) &&
|
||||
(frame->data[1] == 0x10u) &&
|
||||
(frame->data[4] == 0u) && (frame->data[5] == 0u) &&
|
||||
(frame->data[6] == 0u) && (frame->data[7] == 0u);
|
||||
}
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user