2111 lines
68 KiB
C++
2111 lines
68 KiB
C++
/* Compatibility implementation for Seat_Controller_demo.
|
|
*
|
|
* This file is included into the generated production application translation
|
|
* unit. It intentionally reuses motorRuntime/motorProfiles, Motor_ServiceAll
|
|
* and OtaCan. There is one CAN0 driver, one RX interrupt and one low-level
|
|
* motor state machine. The Demo 0x300 command lifetime is bounded by its own
|
|
* target/duration/timeout and safety events; it never depends on 0x65 or any
|
|
* other heartbeat. */
|
|
|
|
#include "seat_legacy_version_query.h"
|
|
|
|
#define SEAT_DEMO_CAN_COMMAND_ID (0x300ul)
|
|
#define SEAT_DEMO_CAN_SYSTEM_STATUS_ID (0x301ul)
|
|
#define SEAT_DEMO_CAN_MOTOR_STATUS_ID (0x302ul)
|
|
#define SEAT_DEMO_CAN_DIAGNOSTIC_ID (0x303ul)
|
|
#define SEAT_DEMO_CAN_CURRENT_DIAGNOSTIC_ID (0x304ul)
|
|
#define SEAT_DEMO_CAN_THERMAL_DIAGNOSTIC_ID (0x305ul)
|
|
#define SEAT_DEMO_CAN_THERMAL_FEEDBACK_ID (0x4FAul)
|
|
#define SEAT_DEMO_CAN_PROTOCOL_VERSION (1u)
|
|
|
|
#define SEAT_DEMO_MOTOR_COUNT (4u)
|
|
#define SEAT_DEMO_ACTIVE_MASK (0x0Fu)
|
|
#define SEAT_DEMO_NORMALIZED_MAX (10000u)
|
|
#define SEAT_DEMO_START_STAGGER_MS (100ul)
|
|
#define SEAT_DEMO_RETRY_DELAY_MS (1000ul)
|
|
#define SEAT_DEMO_TARGET_TOLERANCE_HALL (2ul)
|
|
#define SEAT_DEMO_MIN_DURATION_MS (100ul)
|
|
#define SEAT_DEMO_MAX_DURATION_MS (60000ul)
|
|
#define SEAT_DEMO_MIN_CALIBRATION_SECONDS (10ul)
|
|
#define SEAT_DEMO_MAX_CALIBRATION_SECONDS (600ul)
|
|
#define SEAT_DEMO_CALIBRATION_KEY (0xA55Au)
|
|
#define SEAT_DEMO_HOME_KEY (0x4B52u)
|
|
#define SEAT_DEMO_CLEAR_FAULT_KEY (0xC33Cu)
|
|
#define SEAT_DEMO_TIMED_DUTY_X10_MIN (300u)
|
|
#define SEAT_DEMO_TIMED_DUTY_X10_MAX (1000u)
|
|
#define SEAT_DEMO_SYSTEM_STATUS_MS (100ul)
|
|
#define SEAT_DEMO_MOTOR_POLL_MS (25ul)
|
|
#define SEAT_DEMO_DIAGNOSTIC_POLL_MS (25ul)
|
|
#define SEAT_DEMO_DEFAULT_DUTY (90u)
|
|
#define SEAT_DEMO_CURRENT_THRESHOLD_RAW (500u)
|
|
|
|
typedef enum
|
|
{
|
|
SEAT_DEMO_ACK_NONE = 0u,
|
|
SEAT_DEMO_ACK_ACCEPTED = 1u,
|
|
SEAT_DEMO_ACK_REJECTED = 2u,
|
|
SEAT_DEMO_ACK_DUPLICATE = 3u,
|
|
SEAT_DEMO_ACK_STOP_ALL_ACCEPTED = 4u
|
|
} seat_demo_ack_t;
|
|
|
|
typedef enum
|
|
{
|
|
SEAT_DEMO_SYSTEM_INIT = 0u,
|
|
SEAT_DEMO_SYSTEM_READY = 1u,
|
|
SEAT_DEMO_SYSTEM_ACTIVE = 2u,
|
|
SEAT_DEMO_SYSTEM_INHIBITED = 3u,
|
|
SEAT_DEMO_SYSTEM_FAULT = 4u,
|
|
SEAT_DEMO_SYSTEM_CALIBRATION = 5u
|
|
} seat_demo_system_state_t;
|
|
|
|
typedef enum
|
|
{
|
|
SEAT_DEMO_REJECT_NONE = 0u,
|
|
SEAT_DEMO_REJECT_BAD_DLC = 1u,
|
|
SEAT_DEMO_REJECT_BAD_VERSION = 2u,
|
|
SEAT_DEMO_REJECT_BAD_OPCODE = 3u,
|
|
SEAT_DEMO_REJECT_BAD_MOTOR = 4u,
|
|
SEAT_DEMO_REJECT_BAD_DIRECTION = 5u,
|
|
SEAT_DEMO_REJECT_PARAM_RANGE = 6u,
|
|
SEAT_DEMO_REJECT_POSITION_INVALID = 7u,
|
|
SEAT_DEMO_REJECT_SEQUENCE_OLD = 8u,
|
|
SEAT_DEMO_REJECT_BUSY_SAME_MOTOR = 9u,
|
|
SEAT_DEMO_REJECT_INTERLOCK = 10u,
|
|
SEAT_DEMO_REJECT_FAULT_ACTIVE = 11u,
|
|
SEAT_DEMO_REJECT_SERVICE_MODE_REQUIRED = 12u,
|
|
SEAT_DEMO_REJECT_CAL_DATA_INVALID = 13u,
|
|
SEAT_DEMO_REJECT_INTERNAL_ERROR = 14u
|
|
} seat_demo_reject_t;
|
|
|
|
typedef enum
|
|
{
|
|
SEAT_DEMO_OPCODE_INVALID = 0x00u,
|
|
SEAT_DEMO_OPCODE_MOVE_ABSOLUTE = 0x01u,
|
|
SEAT_DEMO_OPCODE_MOVE_RELATIVE = 0x02u,
|
|
SEAT_DEMO_OPCODE_MOVE_TIMED = 0x03u,
|
|
SEAT_DEMO_OPCODE_STOP_MOTOR = 0x08u,
|
|
SEAT_DEMO_OPCODE_STOP_ALL = 0x09u,
|
|
SEAT_DEMO_OPCODE_CALIBRATE_RANGE = 0x0Au,
|
|
SEAT_DEMO_OPCODE_HOME_KNOWN_RANGE = 0x0Bu,
|
|
SEAT_DEMO_OPCODE_QUERY_STATUS = 0x0Cu,
|
|
SEAT_DEMO_OPCODE_CLEAR_FAULT = 0x0Du,
|
|
SEAT_DEMO_OPCODE_START_DYNAMIC_MODE = 0x0Eu,
|
|
SEAT_DEMO_OPCODE_STOP_DYNAMIC_MODE = 0x0Fu
|
|
} seat_demo_opcode_t;
|
|
|
|
typedef enum
|
|
{
|
|
SEAT_DEMO_TASK_IDLE = 0u,
|
|
SEAT_DEMO_TASK_WAIT_START,
|
|
SEAT_DEMO_TASK_POSITION,
|
|
SEAT_DEMO_TASK_TIMED,
|
|
SEAT_DEMO_TASK_RETRY_DELAY,
|
|
SEAT_DEMO_TASK_CALIBRATION,
|
|
SEAT_DEMO_TASK_COMPLETE,
|
|
SEAT_DEMO_TASK_FAILED
|
|
} seat_demo_task_state_t;
|
|
|
|
typedef enum
|
|
{
|
|
SEAT_DEMO_RESULT_NONE = 0u,
|
|
SEAT_DEMO_RESULT_RUNNING,
|
|
SEAT_DEMO_RESULT_OK,
|
|
SEAT_DEMO_RESULT_STOPPED,
|
|
SEAT_DEMO_RESULT_STALL,
|
|
SEAT_DEMO_RESULT_ENDPOINT,
|
|
SEAT_DEMO_RESULT_TIMEOUT,
|
|
SEAT_DEMO_RESULT_DRIVER_FAULT,
|
|
SEAT_DEMO_RESULT_INVALID,
|
|
SEAT_DEMO_RESULT_CALIBRATION_FAILED
|
|
} seat_demo_result_t;
|
|
|
|
typedef struct
|
|
{
|
|
uint8_t opcode;
|
|
uint8_t motor;
|
|
motor_direction_t direction;
|
|
uint16_t sequence;
|
|
uint16_t paramA;
|
|
uint16_t paramB;
|
|
uint8_t dutyPercent;
|
|
bool explicitDuty;
|
|
} seat_demo_command_t;
|
|
|
|
typedef struct
|
|
{
|
|
seat_demo_command_t command;
|
|
seat_demo_task_state_t state;
|
|
seat_demo_result_t result;
|
|
uint32_t targetValueX100;
|
|
uint32_t targetHallCount;
|
|
uint32_t durationTicks;
|
|
uint32_t deadlineTicks;
|
|
uint32_t retryAtTicks;
|
|
uint32_t rawStartEdges;
|
|
uint8_t dutyPercent;
|
|
uint8_t retryCount;
|
|
bool pending;
|
|
bool active;
|
|
} seat_demo_motor_task_t;
|
|
|
|
static const uint32_t seatDemoDefaultTimeoutMs[SEAT_DEMO_MOTOR_COUNT] =
|
|
{
|
|
12000ul, 8000ul, 12000ul, 10000ul
|
|
};
|
|
|
|
static seat_demo_motor_task_t seatDemoTasks[SEAT_DEMO_MOTOR_COUNT];
|
|
static seat_demo_command_t seatDemoLastCommand;
|
|
static bool seatDemoInitialized;
|
|
static bool seatDemoLastSequenceValid;
|
|
static bool seatDemoLastCommandValid;
|
|
static bool seatDemoAckNeedsClear;
|
|
static bool seatDemoCalibrationWorker;
|
|
static uint8_t seatDemoCalibrationAxis = 0xFFu;
|
|
static uint8_t seatDemoServiceDepth;
|
|
static uint8_t seatDemoAck;
|
|
static uint8_t seatDemoReject;
|
|
static uint8_t seatDemoLastOpcode;
|
|
static uint16_t seatDemoLastSequence;
|
|
static uint16_t seatDemoSequenceReference;
|
|
static uint8_t seatDemoPendingMask;
|
|
static uint8_t seatDemoMotorStatusPendingMask;
|
|
static uint8_t seatDemoDiagnosticPendingMask;
|
|
static uint8_t seatDemoDispatchCursor;
|
|
static uint8_t seatDemoMotorPollCursor;
|
|
static uint8_t seatDemoDiagnosticPollCursor;
|
|
static uint8_t seatDemoMotorPeriodicId;
|
|
static uint8_t seatDemoDiagnosticPeriodicId;
|
|
static bool seatDemoMotorPeriodicPending;
|
|
static bool seatDemoDiagnosticPeriodicPending;
|
|
static bool seatDemoSystemStatusPending;
|
|
static uint8_t seatDemoSystemAlive;
|
|
static uint8_t seatDemoDiagnosticAlive[SEAT_DEMO_MOTOR_COUNT];
|
|
static uint32_t seatDemoNextStartTicks;
|
|
static uint32_t seatDemoNextSystemStatusTicks;
|
|
static uint32_t seatDemoNextMotorPollTicks;
|
|
static uint32_t seatDemoNextDiagnosticPollTicks;
|
|
static volatile bool seatDemoStopAllFromIsr;
|
|
static volatile uint16_t seatDemoStopAllSequenceFromIsr;
|
|
|
|
static void SeatDemoDynamic_Init(void);
|
|
static bool SeatDemoDynamic_Start(uint8_t mode, uint16_t sequence);
|
|
static void SeatDemoDynamic_Stop(uint16_t sequence);
|
|
static void SeatDemoDynamic_Service(void);
|
|
static bool SeatDemoDynamic_SendStatus(void);
|
|
static bool SeatDemoDynamic_IsActive(void);
|
|
static uint8_t SeatDemoDynamic_ActiveMask(void);
|
|
static uint8_t SeatDemoDynamic_FaultMask(void);
|
|
static uint8_t SeatDemoCompat_PositionValidMask(void);
|
|
|
|
/* ADC values are populated by seat_demo_thermal.inc and published unchanged
|
|
* in Demo 0x304/0x305 layouts. */
|
|
static bool seatDemoCurrentSampleValid[6];
|
|
static bool seatDemoCurrentBaselineValid[SEAT_DEMO_MOTOR_COUNT];
|
|
static bool seatDemoCurrentHigh[SEAT_DEMO_MOTOR_COUNT];
|
|
static uint16_t seatDemoCurrentRaw[6];
|
|
static uint16_t seatDemoCurrentBaseline[SEAT_DEMO_MOTOR_COUNT];
|
|
static uint16_t seatDemoCurrentThreshold[SEAT_DEMO_MOTOR_COUNT];
|
|
|
|
static bool SeatDemoCompat_TimeReached(uint32_t now, uint32_t deadline)
|
|
{
|
|
return ((int32_t)(now - deadline) >= 0);
|
|
}
|
|
|
|
static uint32_t SeatDemoCompat_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 SeatDemoCompat_WriteLe16(uint8_t *data, uint16_t value)
|
|
{
|
|
data[0] = (uint8_t)value;
|
|
data[1] = (uint8_t)(value >> 8u);
|
|
}
|
|
|
|
static bool SeatDemoCompat_SequenceNewer(uint16_t candidate,
|
|
uint16_t reference)
|
|
{
|
|
return ((int16_t)(candidate - reference) > 0);
|
|
}
|
|
|
|
static bool SeatDemoCompat_CommandEqual(const seat_demo_command_t *lhs,
|
|
const seat_demo_command_t *rhs)
|
|
{
|
|
return (lhs->opcode == rhs->opcode) &&
|
|
(lhs->motor == rhs->motor) &&
|
|
(lhs->direction == rhs->direction) &&
|
|
(lhs->sequence == rhs->sequence) &&
|
|
(lhs->paramA == rhs->paramA) &&
|
|
(lhs->paramB == rhs->paramB) &&
|
|
(lhs->dutyPercent == rhs->dutyPercent) &&
|
|
(lhs->explicitDuty == rhs->explicitDuty);
|
|
}
|
|
|
|
static bool SeatDemoCompat_MotorSettled(uint32_t axis)
|
|
{
|
|
uint32_t interruptState;
|
|
bool settled;
|
|
|
|
interruptState = Cy_SysLib_EnterCriticalSection();
|
|
settled = (motorRuntime[axis].direction == MOTOR_DIRECTION_STOPPED) &&
|
|
(false == motorRuntime[axis].startPending) &&
|
|
(false == motorRuntime[axis].targetActive) &&
|
|
(false == motorRuntime[axis].coastTracking);
|
|
Cy_SysLib_ExitCriticalSection(interruptState);
|
|
return settled;
|
|
}
|
|
|
|
static uint8_t SeatDemoCompat_ActiveMask(void)
|
|
{
|
|
uint32_t axis;
|
|
uint8_t mask = (uint8_t)
|
|
(seatDemoPendingMask | SeatDemoDynamic_ActiveMask());
|
|
|
|
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
|
{
|
|
if(false != seatDemoTasks[axis].active)
|
|
{
|
|
mask |= (uint8_t)(1u << axis);
|
|
}
|
|
}
|
|
return mask;
|
|
}
|
|
|
|
static uint8_t SeatDemoCompat_ActiveCount(void)
|
|
{
|
|
uint8_t mask = SeatDemoCompat_ActiveMask();
|
|
uint8_t count = 0u;
|
|
|
|
while(mask != 0u)
|
|
{
|
|
count += mask & 1u;
|
|
mask >>= 1u;
|
|
}
|
|
return count;
|
|
}
|
|
|
|
static bool SeatDemoCompat_CalibrationBusy(void)
|
|
{
|
|
uint32_t axis;
|
|
|
|
if(false != seatDemoCalibrationWorker)
|
|
{
|
|
return true;
|
|
}
|
|
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
|
{
|
|
if(((false != seatDemoTasks[axis].pending) ||
|
|
(false != seatDemoTasks[axis].active)) &&
|
|
((seatDemoTasks[axis].command.opcode ==
|
|
SEAT_DEMO_OPCODE_CALIBRATE_RANGE) ||
|
|
(seatDemoTasks[axis].command.opcode ==
|
|
SEAT_DEMO_OPCODE_HOME_KNOWN_RANGE)))
|
|
{
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
static bool SeatDemoCompat_IsMotionOpcode(uint8_t opcode)
|
|
{
|
|
return (opcode == SEAT_DEMO_OPCODE_MOVE_ABSOLUTE) ||
|
|
(opcode == SEAT_DEMO_OPCODE_MOVE_RELATIVE) ||
|
|
(opcode == SEAT_DEMO_OPCODE_MOVE_TIMED) ||
|
|
(opcode == SEAT_DEMO_OPCODE_CALIBRATE_RANGE) ||
|
|
(opcode == SEAT_DEMO_OPCODE_HOME_KNOWN_RANGE);
|
|
}
|
|
|
|
static bool SeatDemoCompat_IsPositionOpcode(uint8_t opcode)
|
|
{
|
|
return (opcode == SEAT_DEMO_OPCODE_MOVE_ABSOLUTE) ||
|
|
(opcode == SEAT_DEMO_OPCODE_MOVE_RELATIVE);
|
|
}
|
|
|
|
static void SeatDemoCompat_SetResponse(seat_demo_ack_t ack,
|
|
seat_demo_reject_t reject,
|
|
const seat_demo_command_t *command)
|
|
{
|
|
seatDemoAck = (uint8_t)ack;
|
|
seatDemoReject = (uint8_t)reject;
|
|
if(NULL != command)
|
|
{
|
|
seatDemoLastOpcode = command->opcode;
|
|
seatDemoLastSequence = command->sequence;
|
|
}
|
|
seatDemoAckNeedsClear = (ack != SEAT_DEMO_ACK_NONE);
|
|
seatDemoSystemStatusPending = true;
|
|
}
|
|
|
|
static uint8_t SeatDemoCompat_DefaultDuty(uint32_t axis)
|
|
{
|
|
(void)axis;
|
|
return SEAT_DEMO_DEFAULT_DUTY;
|
|
}
|
|
|
|
static uint32_t SeatDemoCompat_CommandTimeoutMs(
|
|
const seat_demo_command_t *command)
|
|
{
|
|
if((command->opcode == SEAT_DEMO_OPCODE_CALIBRATE_RANGE) ||
|
|
(command->opcode == SEAT_DEMO_OPCODE_HOME_KNOWN_RANGE))
|
|
{
|
|
return (uint32_t)command->paramB * 1000ul;
|
|
}
|
|
return (command->paramB != 0u) ? command->paramB :
|
|
seatDemoDefaultTimeoutMs[command->motor];
|
|
}
|
|
|
|
static uint8_t SeatDemoCompat_CommandDuty(
|
|
const seat_demo_command_t *command)
|
|
{
|
|
uint32_t duty;
|
|
|
|
if(false != command->explicitDuty)
|
|
{
|
|
return command->dutyPercent;
|
|
}
|
|
if(command->opcode != SEAT_DEMO_OPCODE_MOVE_TIMED)
|
|
{
|
|
return SeatDemoCompat_DefaultDuty(command->motor);
|
|
}
|
|
duty = ((uint32_t)command->paramB + 5ul) / 10ul;
|
|
if(duty < MOTOR_SPEED_MIN_DUTY_PERCENT)
|
|
{
|
|
duty = MOTOR_SPEED_MIN_DUTY_PERCENT;
|
|
}
|
|
if(duty > MOTOR_SPEED_MAX_DUTY_PERCENT)
|
|
{
|
|
duty = MOTOR_SPEED_MAX_DUTY_PERCENT;
|
|
}
|
|
return (uint8_t)duty;
|
|
}
|
|
|
|
static bool SeatDemoCompat_PositionValid(uint32_t axis)
|
|
{
|
|
uint32_t interruptState;
|
|
bool valid;
|
|
|
|
interruptState = Cy_SysLib_EnterCriticalSection();
|
|
valid = motorRuntime[axis].positionValid &&
|
|
(motorRuntime[axis].maxHallCount != 0ul);
|
|
Cy_SysLib_ExitCriticalSection(interruptState);
|
|
return valid;
|
|
}
|
|
|
|
#include "seat_demo_dynamic.inc"
|
|
|
|
static void SeatDemoCompat_MarkTaskStopped(uint32_t axis,
|
|
uint16_t sequence)
|
|
{
|
|
seat_demo_motor_task_t *task = &seatDemoTasks[axis];
|
|
|
|
if(false != task->command.explicitDuty)
|
|
{
|
|
(void)Motor_SetDutyPercent(
|
|
(motor_mode_t)axis, SeatDemoCompat_DefaultDuty(axis));
|
|
}
|
|
task->pending = false;
|
|
task->active = false;
|
|
task->state = SEAT_DEMO_TASK_IDLE;
|
|
task->result = SEAT_DEMO_RESULT_STOPPED;
|
|
task->command.sequence = sequence;
|
|
seatDemoPendingMask &= (uint8_t)(~(uint8_t)(1u << axis));
|
|
seatDemoMotorStatusPendingMask |= (uint8_t)(1u << axis);
|
|
seatDemoDiagnosticPendingMask |= (uint8_t)(1u << axis);
|
|
seatDemoSystemStatusPending = true;
|
|
}
|
|
|
|
static void SeatDemoCompat_StopAxis(uint32_t axis, uint16_t sequence)
|
|
{
|
|
if(axis >= SEAT_DEMO_MOTOR_COUNT)
|
|
{
|
|
return;
|
|
}
|
|
if((false != seatDemoTasks[axis].active) ||
|
|
(false != seatDemoTasks[axis].pending))
|
|
{
|
|
(void)Motor_StopMode((motor_mode_t)axis);
|
|
}
|
|
SeatDemoCompat_MarkTaskStopped(axis, sequence);
|
|
}
|
|
|
|
static void SeatDemoCompat_StopAllInternal(uint16_t sequence)
|
|
{
|
|
uint32_t axis;
|
|
|
|
SeatDemoDynamic_Stop(sequence);
|
|
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
|
{
|
|
(void)Motor_StopMode((motor_mode_t)axis);
|
|
SeatDemoCompat_MarkTaskStopped(axis, sequence);
|
|
}
|
|
}
|
|
|
|
static bool SeatDemoCompat_TargetWithinTolerance(uint32_t axis)
|
|
{
|
|
uint32_t position;
|
|
uint32_t target = seatDemoTasks[axis].targetHallCount;
|
|
uint32_t difference;
|
|
uint32_t interruptState = Cy_SysLib_EnterCriticalSection();
|
|
|
|
position = motorRuntime[axis].positionHallCount;
|
|
Cy_SysLib_ExitCriticalSection(interruptState);
|
|
difference = (position >= target) ? (position - target) :
|
|
(target - position);
|
|
return (difference <= SEAT_DEMO_TARGET_TOLERANCE_HALL);
|
|
}
|
|
|
|
static void SeatDemoCompat_SetTaskComplete(uint32_t axis)
|
|
{
|
|
seat_demo_motor_task_t *task = &seatDemoTasks[axis];
|
|
|
|
if(false != task->command.explicitDuty)
|
|
{
|
|
(void)Motor_SetDutyPercent(
|
|
(motor_mode_t)axis, SeatDemoCompat_DefaultDuty(axis));
|
|
}
|
|
task->active = false;
|
|
task->pending = false;
|
|
task->state = SEAT_DEMO_TASK_COMPLETE;
|
|
task->result = SEAT_DEMO_RESULT_OK;
|
|
seatDemoPendingMask &= (uint8_t)(~(uint8_t)(1u << axis));
|
|
seatDemoMotorStatusPendingMask |= (uint8_t)(1u << axis);
|
|
seatDemoDiagnosticPendingMask |= (uint8_t)(1u << axis);
|
|
seatDemoSystemStatusPending = true;
|
|
otaSeatPersistedMask &= ~(1ul << axis);
|
|
(void)OtaSeat_SavePersistentState();
|
|
}
|
|
|
|
static void SeatDemoCompat_SetTaskFailed(uint32_t axis,
|
|
seat_demo_result_t result)
|
|
{
|
|
seat_demo_motor_task_t *task = &seatDemoTasks[axis];
|
|
|
|
(void)Motor_StopMode((motor_mode_t)axis);
|
|
if(false != task->command.explicitDuty)
|
|
{
|
|
(void)Motor_SetDutyPercent(
|
|
(motor_mode_t)axis, SeatDemoCompat_DefaultDuty(axis));
|
|
}
|
|
task->active = false;
|
|
task->pending = false;
|
|
task->state = SEAT_DEMO_TASK_FAILED;
|
|
task->result = result;
|
|
seatDemoPendingMask &= (uint8_t)(~(uint8_t)(1u << axis));
|
|
seatDemoMotorStatusPendingMask |= (uint8_t)(1u << axis);
|
|
seatDemoDiagnosticPendingMask |= (uint8_t)(1u << axis);
|
|
seatDemoSystemStatusPending = true;
|
|
}
|
|
|
|
static bool SeatDemoCompat_ParseCommand(const ota_can_frame_t *frame,
|
|
seat_demo_command_t *command,
|
|
seat_demo_reject_t *reject)
|
|
{
|
|
uint8_t control;
|
|
|
|
command->opcode = frame->data[0];
|
|
control = frame->data[1];
|
|
command->motor = control & 0x03u;
|
|
command->direction = (motor_direction_t)((control >> 2u) & 0x03u);
|
|
command->sequence = (uint16_t)((uint16_t)frame->data[2] |
|
|
((uint16_t)frame->data[3] << 8u));
|
|
command->paramA = (uint16_t)((uint16_t)frame->data[4] |
|
|
((uint16_t)frame->data[5] << 8u));
|
|
command->paramB = (uint16_t)((uint16_t)frame->data[6] |
|
|
((uint16_t)frame->data[7] << 8u));
|
|
|
|
if((frame->identifier != SEAT_DEMO_CAN_COMMAND_ID) ||
|
|
(frame->dlc != 8u))
|
|
{
|
|
*reject = SEAT_DEMO_REJECT_BAD_DLC;
|
|
return false;
|
|
}
|
|
if((((control >> 4u) & 0x03u) != SEAT_DEMO_CAN_PROTOCOL_VERSION) ||
|
|
((control & 0xC0u) != 0u))
|
|
{
|
|
*reject = SEAT_DEMO_REJECT_BAD_VERSION;
|
|
return false;
|
|
}
|
|
switch((seat_demo_opcode_t)command->opcode)
|
|
{
|
|
case SEAT_DEMO_OPCODE_MOVE_ABSOLUTE:
|
|
case SEAT_DEMO_OPCODE_MOVE_RELATIVE:
|
|
case SEAT_DEMO_OPCODE_MOVE_TIMED:
|
|
case SEAT_DEMO_OPCODE_STOP_MOTOR:
|
|
case SEAT_DEMO_OPCODE_STOP_ALL:
|
|
case SEAT_DEMO_OPCODE_CALIBRATE_RANGE:
|
|
case SEAT_DEMO_OPCODE_HOME_KNOWN_RANGE:
|
|
case SEAT_DEMO_OPCODE_QUERY_STATUS:
|
|
case SEAT_DEMO_OPCODE_CLEAR_FAULT:
|
|
case SEAT_DEMO_OPCODE_START_DYNAMIC_MODE:
|
|
case SEAT_DEMO_OPCODE_STOP_DYNAMIC_MODE:
|
|
break;
|
|
default:
|
|
*reject = SEAT_DEMO_REJECT_BAD_OPCODE;
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
static bool SeatDemoCompat_ParametersValid(
|
|
const seat_demo_command_t *command,
|
|
seat_demo_reject_t *reject)
|
|
{
|
|
int32_t relative;
|
|
|
|
if(command->motor >= SEAT_DEMO_MOTOR_COUNT)
|
|
{
|
|
*reject = SEAT_DEMO_REJECT_BAD_MOTOR;
|
|
return false;
|
|
}
|
|
if(((command->opcode == SEAT_DEMO_OPCODE_STOP_ALL) ||
|
|
(command->opcode == SEAT_DEMO_OPCODE_START_DYNAMIC_MODE) ||
|
|
(command->opcode == SEAT_DEMO_OPCODE_STOP_DYNAMIC_MODE)) &&
|
|
(command->motor != 0u))
|
|
{
|
|
*reject = SEAT_DEMO_REJECT_BAD_MOTOR;
|
|
return false;
|
|
}
|
|
if(command->opcode == SEAT_DEMO_OPCODE_MOVE_TIMED)
|
|
{
|
|
if((command->direction != MOTOR_DIRECTION_TOWARD_FAR) &&
|
|
(command->direction != MOTOR_DIRECTION_TOWARD_NEAR))
|
|
{
|
|
*reject = SEAT_DEMO_REJECT_BAD_DIRECTION;
|
|
return false;
|
|
}
|
|
}
|
|
else if(command->direction != MOTOR_DIRECTION_STOPPED)
|
|
{
|
|
*reject = SEAT_DEMO_REJECT_BAD_DIRECTION;
|
|
return false;
|
|
}
|
|
|
|
switch((seat_demo_opcode_t)command->opcode)
|
|
{
|
|
case SEAT_DEMO_OPCODE_MOVE_ABSOLUTE:
|
|
if((command->paramA > SEAT_DEMO_NORMALIZED_MAX) ||
|
|
((command->paramB != 0u) &&
|
|
((command->paramB < SEAT_DEMO_MIN_DURATION_MS) ||
|
|
(command->paramB > SEAT_DEMO_MAX_DURATION_MS))))
|
|
{
|
|
*reject = SEAT_DEMO_REJECT_PARAM_RANGE;
|
|
return false;
|
|
}
|
|
break;
|
|
case SEAT_DEMO_OPCODE_MOVE_RELATIVE:
|
|
relative = (int16_t)command->paramA;
|
|
if((relative < -10000) || (relative > 10000) ||
|
|
((command->paramB != 0u) &&
|
|
((command->paramB < SEAT_DEMO_MIN_DURATION_MS) ||
|
|
(command->paramB > SEAT_DEMO_MAX_DURATION_MS))))
|
|
{
|
|
*reject = SEAT_DEMO_REJECT_PARAM_RANGE;
|
|
return false;
|
|
}
|
|
break;
|
|
case SEAT_DEMO_OPCODE_MOVE_TIMED:
|
|
if((command->paramA < SEAT_DEMO_MIN_DURATION_MS) ||
|
|
(command->paramA > SEAT_DEMO_MAX_DURATION_MS) ||
|
|
(command->paramB < SEAT_DEMO_TIMED_DUTY_X10_MIN) ||
|
|
(command->paramB > SEAT_DEMO_TIMED_DUTY_X10_MAX))
|
|
{
|
|
*reject = SEAT_DEMO_REJECT_PARAM_RANGE;
|
|
return false;
|
|
}
|
|
break;
|
|
case SEAT_DEMO_OPCODE_STOP_MOTOR:
|
|
case SEAT_DEMO_OPCODE_STOP_ALL:
|
|
case SEAT_DEMO_OPCODE_QUERY_STATUS:
|
|
if((command->paramA != 0u) || (command->paramB != 0u))
|
|
{
|
|
*reject = SEAT_DEMO_REJECT_PARAM_RANGE;
|
|
return false;
|
|
}
|
|
break;
|
|
case SEAT_DEMO_OPCODE_CALIBRATE_RANGE:
|
|
if((command->paramA != SEAT_DEMO_CALIBRATION_KEY) ||
|
|
(command->paramB < SEAT_DEMO_MIN_CALIBRATION_SECONDS) ||
|
|
(command->paramB > SEAT_DEMO_MAX_CALIBRATION_SECONDS))
|
|
{
|
|
*reject = SEAT_DEMO_REJECT_PARAM_RANGE;
|
|
return false;
|
|
}
|
|
break;
|
|
case SEAT_DEMO_OPCODE_HOME_KNOWN_RANGE:
|
|
if((command->paramA != SEAT_DEMO_HOME_KEY) ||
|
|
(command->paramB < SEAT_DEMO_MIN_CALIBRATION_SECONDS) ||
|
|
(command->paramB > SEAT_DEMO_MAX_CALIBRATION_SECONDS))
|
|
{
|
|
*reject = SEAT_DEMO_REJECT_PARAM_RANGE;
|
|
return false;
|
|
}
|
|
break;
|
|
case SEAT_DEMO_OPCODE_CLEAR_FAULT:
|
|
if((command->paramA != SEAT_DEMO_CLEAR_FAULT_KEY) ||
|
|
(command->paramB != 0u))
|
|
{
|
|
*reject = SEAT_DEMO_REJECT_PARAM_RANGE;
|
|
return false;
|
|
}
|
|
break;
|
|
case SEAT_DEMO_OPCODE_START_DYNAMIC_MODE:
|
|
if(((command->paramA !=
|
|
SEAT_DEMO_DYNAMIC_MODE_ROCKING_HORSE) &&
|
|
(command->paramA != SEAT_DEMO_DYNAMIC_MODE_CRADLE)) ||
|
|
(command->paramB != 0u))
|
|
{
|
|
*reject = SEAT_DEMO_REJECT_PARAM_RANGE;
|
|
return false;
|
|
}
|
|
break;
|
|
case SEAT_DEMO_OPCODE_STOP_DYNAMIC_MODE:
|
|
if((command->paramA != 0u) || (command->paramB != 0u))
|
|
{
|
|
*reject = SEAT_DEMO_REJECT_PARAM_RANGE;
|
|
return false;
|
|
}
|
|
break;
|
|
default:
|
|
*reject = SEAT_DEMO_REJECT_BAD_OPCODE;
|
|
return false;
|
|
}
|
|
*reject = SEAT_DEMO_REJECT_NONE;
|
|
return true;
|
|
}
|
|
|
|
static bool SeatDemoCompat_NativeJobBlocksMotion(uint32_t axis,
|
|
bool calibrationCommand)
|
|
{
|
|
#if (MOTOR_OTA_CALIBRATION != 0u)
|
|
uint32_t bit = 1ul << axis;
|
|
|
|
if((otaCalibrationPendingJob != OTA_SEAT_JOB_NONE) ||
|
|
((false != otaSeatJobActive) &&
|
|
(otaSeatJobType != OTA_SEAT_JOB_MOVE) &&
|
|
(otaSeatJobType != OTA_SEAT_JOB_DYNAMIC)))
|
|
{
|
|
return true;
|
|
}
|
|
if(false != calibrationCommand)
|
|
{
|
|
return (otaCalibrationMoveActiveMask != 0ul) ||
|
|
(false != otaCalibrationDynamicActive);
|
|
}
|
|
if(false != otaCalibrationDynamicActive)
|
|
{
|
|
OtaCalibration_StopDynamic(OTA_CALIBRATION_ERROR_NONE, true);
|
|
}
|
|
if((otaCalibrationMoveActiveMask & bit) != 0ul)
|
|
{
|
|
OtaCalibration_FinalizeInteractiveAxis(
|
|
axis, OTA_CALIBRATION_ERROR_NONE, true, true);
|
|
}
|
|
#else
|
|
(void)axis;
|
|
(void)calibrationCommand;
|
|
#endif
|
|
|
|
#if (MOTOR_OTA_SELF_COMPOSE != 0u)
|
|
if(false != otaSelfActive)
|
|
{
|
|
OtaSelf_Abort();
|
|
}
|
|
#endif
|
|
return false;
|
|
}
|
|
|
|
static void SeatDemoCompat_QueueCommand(const seat_demo_command_t *command)
|
|
{
|
|
uint32_t axis = command->motor;
|
|
seat_demo_motor_task_t *task = &seatDemoTasks[axis];
|
|
|
|
task->command = *command;
|
|
task->pending = true;
|
|
task->active = false;
|
|
task->state = SEAT_DEMO_TASK_WAIT_START;
|
|
task->result = SEAT_DEMO_RESULT_RUNNING;
|
|
task->retryCount = 0u;
|
|
task->deadlineTicks = 0ul;
|
|
seatDemoPendingMask |= (uint8_t)(1u << axis);
|
|
seatDemoMotorStatusPendingMask |= (uint8_t)(1u << axis);
|
|
seatDemoDiagnosticPendingMask |= (uint8_t)(1u << axis);
|
|
seatDemoSystemStatusPending = true;
|
|
}
|
|
|
|
static bool SeatDemoCompat_StartPositionTask(uint32_t axis,
|
|
seat_demo_motor_task_t *task)
|
|
{
|
|
motor_position_info_t info;
|
|
int32_t targetNormalized;
|
|
uint32_t targetValueX100;
|
|
uint32_t timeoutMs = SeatDemoCompat_CommandTimeoutMs(&task->command);
|
|
motor_safety_options_t safety = { false, false };
|
|
|
|
if((MOTOR_API_SUCCESS != Motor_GetPositionInfo(
|
|
(motor_mode_t)axis, &info)) ||
|
|
(false == info.positionValid) || (0ul == info.maxValueX100))
|
|
{
|
|
return false;
|
|
}
|
|
if(task->command.opcode == SEAT_DEMO_OPCODE_MOVE_ABSOLUTE)
|
|
{
|
|
targetNormalized = (int32_t)task->command.paramA;
|
|
}
|
|
else
|
|
{
|
|
targetNormalized = (int32_t)info.percentX100 +
|
|
(int16_t)task->command.paramA;
|
|
if(targetNormalized < 0)
|
|
{
|
|
targetNormalized = 0;
|
|
}
|
|
else if(targetNormalized > 10000)
|
|
{
|
|
targetNormalized = 10000;
|
|
}
|
|
}
|
|
targetValueX100 = (uint32_t)
|
|
(((uint64_t)(uint32_t)targetNormalized * info.maxValueX100 +
|
|
5000ull) / 10000ull);
|
|
|
|
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, task->dutyPercent)))
|
|
{
|
|
return false;
|
|
}
|
|
if(MOTOR_API_SUCCESS != Motor_StartMoveToValue(
|
|
(motor_mode_t)axis, targetValueX100))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
task->targetValueX100 = targetValueX100;
|
|
task->targetHallCount = (uint32_t)
|
|
(((uint64_t)targetValueX100 * info.maxHallCount +
|
|
(info.maxValueX100 / 2ul)) / info.maxValueX100);
|
|
task->durationTicks = MOTOR_MS_TO_TICKS(timeoutMs);
|
|
task->deadlineTicks = 0ul;
|
|
task->state = SEAT_DEMO_TASK_WAIT_START;
|
|
task->result = SEAT_DEMO_RESULT_RUNNING;
|
|
task->active = true;
|
|
otaSeatPersistedMask &= ~(1ul << axis);
|
|
return true;
|
|
}
|
|
|
|
static bool SeatDemoCompat_StartTimedTask(uint32_t axis,
|
|
seat_demo_motor_task_t *task)
|
|
{
|
|
motor_safety_options_t safety = { false, false };
|
|
uint32_t interruptState;
|
|
|
|
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, task->dutyPercent)))
|
|
{
|
|
return false;
|
|
}
|
|
interruptState = Cy_SysLib_EnterCriticalSection();
|
|
task->rawStartEdges = motorRuntime[axis].rawRisingEdges;
|
|
Cy_SysLib_ExitCriticalSection(interruptState);
|
|
if(MOTOR_API_SUCCESS != Motor_StartDirectionInternal(
|
|
(motor_mode_t)axis,
|
|
task->command.direction,
|
|
false,
|
|
0ul,
|
|
false,
|
|
true,
|
|
MOTOR_DIRECTION_STOPPED,
|
|
false))
|
|
{
|
|
return false;
|
|
}
|
|
task->durationTicks = MOTOR_MS_TO_TICKS(task->command.paramA);
|
|
task->deadlineTicks = 0ul;
|
|
task->state = SEAT_DEMO_TASK_WAIT_START;
|
|
task->result = SEAT_DEMO_RESULT_RUNNING;
|
|
task->active = true;
|
|
otaSeatPersistedMask &= ~(1ul << axis);
|
|
return true;
|
|
}
|
|
|
|
static bool SeatDemoCompat_StartCalibrationTask(
|
|
uint32_t axis,
|
|
seat_demo_motor_task_t *task)
|
|
{
|
|
motor_calibration_result_t result;
|
|
motor_calibration_action_t action;
|
|
uint32_t timeoutMs = SeatDemoCompat_CommandTimeoutMs(&task->command);
|
|
uint32_t totalLegs;
|
|
uint8_t duty;
|
|
uint8_t status;
|
|
|
|
action = (task->command.opcode == SEAT_DEMO_OPCODE_CALIBRATE_RANGE) ?
|
|
MOTOR_CALIBRATE_FIND_BOTH : MOTOR_CALIBRATE_FIND_NEAR;
|
|
totalLegs = (action == MOTOR_CALIBRATE_FIND_BOTH) ? 3ul : 1ul;
|
|
duty = (action == MOTOR_CALIBRATE_FIND_BOTH) ?
|
|
(uint8_t)MOTOR_SPEED_MIN_DUTY_PERCENT :
|
|
SeatDemoCompat_DefaultDuty(axis);
|
|
if((MOTOR_API_SUCCESS != Motor_DisableSpeedControl(
|
|
(motor_mode_t)axis)) ||
|
|
(MOTOR_API_SUCCESS != Motor_SetDutyPercent(
|
|
(motor_mode_t)axis, duty)))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
task->state = SEAT_DEMO_TASK_CALIBRATION;
|
|
task->result = SEAT_DEMO_RESULT_RUNNING;
|
|
task->active = true;
|
|
task->durationTicks = MOTOR_MS_TO_TICKS(timeoutMs * totalLegs);
|
|
task->deadlineTicks = hallTimebaseTicks + task->durationTicks;
|
|
seatDemoCalibrationAxis = (uint8_t)axis;
|
|
seatDemoCalibrationWorker = true;
|
|
seatDemoMotorStatusPendingMask |= (uint8_t)(1u << axis);
|
|
seatDemoSystemStatusPending = true;
|
|
status = Motor_Calibrate((motor_mode_t)axis, action, &result);
|
|
seatDemoCalibrationWorker = false;
|
|
seatDemoCalibrationAxis = 0xFFu;
|
|
|
|
if(false == task->active)
|
|
{
|
|
return true;
|
|
}
|
|
if((MOTOR_API_SUCCESS != status) ||
|
|
(false == result.nearEndpointFound) ||
|
|
((action == MOTOR_CALIBRATE_FIND_BOTH) &&
|
|
(false == result.farEndpointFound)))
|
|
{
|
|
SeatDemoCompat_SetTaskFailed(
|
|
axis, SEAT_DEMO_RESULT_CALIBRATION_FAILED);
|
|
return false;
|
|
}
|
|
SeatDemoCompat_SetTaskComplete(axis);
|
|
return true;
|
|
}
|
|
|
|
static bool SeatDemoCompat_DispatchTask(uint32_t axis)
|
|
{
|
|
seat_demo_motor_task_t *task = &seatDemoTasks[axis];
|
|
seat_demo_command_t command = task->command;
|
|
bool calibrationCommand =
|
|
(command.opcode == SEAT_DEMO_OPCODE_CALIBRATE_RANGE) ||
|
|
(command.opcode == SEAT_DEMO_OPCODE_HOME_KNOWN_RANGE);
|
|
|
|
task->pending = false;
|
|
seatDemoPendingMask &= (uint8_t)(~(uint8_t)(1u << axis));
|
|
if(false != SeatDemoCompat_NativeJobBlocksMotion(
|
|
axis, calibrationCommand))
|
|
{
|
|
return false;
|
|
}
|
|
Cy_GPIO_Clr(DRV_DRVOFF_PORT, DRV_DRVOFF_PIN);
|
|
Cy_SysTick_DelayInUs(100ul);
|
|
task->dutyPercent = SeatDemoCompat_CommandDuty(&command);
|
|
|
|
if(false != SeatDemoCompat_IsPositionOpcode(command.opcode))
|
|
{
|
|
return SeatDemoCompat_StartPositionTask(axis, task);
|
|
}
|
|
if(command.opcode == SEAT_DEMO_OPCODE_MOVE_TIMED)
|
|
{
|
|
return SeatDemoCompat_StartTimedTask(axis, task);
|
|
}
|
|
return SeatDemoCompat_StartCalibrationTask(axis, task);
|
|
}
|
|
|
|
static void SeatDemoCompat_ServiceDispatcher(void)
|
|
{
|
|
uint32_t now = hallTimebaseTicks;
|
|
uint32_t checked;
|
|
|
|
if((false != seatDemoCalibrationWorker) ||
|
|
(false == SeatDemoCompat_TimeReached(now, seatDemoNextStartTicks)))
|
|
{
|
|
return;
|
|
}
|
|
for(checked = 0ul; checked < SEAT_DEMO_MOTOR_COUNT; checked++)
|
|
{
|
|
uint32_t axis = (seatDemoDispatchCursor + checked) %
|
|
SEAT_DEMO_MOTOR_COUNT;
|
|
if(false != seatDemoTasks[axis].pending)
|
|
{
|
|
seatDemoDispatchCursor = (uint8_t)
|
|
((axis + 1ul) % SEAT_DEMO_MOTOR_COUNT);
|
|
seatDemoNextStartTicks = now +
|
|
MOTOR_MS_TO_TICKS(SEAT_DEMO_START_STAGGER_MS);
|
|
if(false == SeatDemoCompat_DispatchTask(axis))
|
|
{
|
|
if(false != seatDemoTasks[axis].active)
|
|
{
|
|
SeatDemoCompat_SetTaskFailed(
|
|
axis, SEAT_DEMO_RESULT_INVALID);
|
|
}
|
|
else
|
|
{
|
|
seatDemoTasks[axis].state = SEAT_DEMO_TASK_FAILED;
|
|
seatDemoTasks[axis].result = SEAT_DEMO_RESULT_INVALID;
|
|
seatDemoMotorStatusPendingMask |=
|
|
(uint8_t)(1u << axis);
|
|
seatDemoDiagnosticPendingMask |=
|
|
(uint8_t)(1u << axis);
|
|
seatDemoSystemStatusPending = true;
|
|
}
|
|
SeatDemoCompat_SetResponse(
|
|
SEAT_DEMO_ACK_REJECTED,
|
|
SEAT_DEMO_REJECT_INTERNAL_ERROR,
|
|
&seatDemoTasks[axis].command);
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
static void SeatDemoCompat_StopNativeOwnersForAll(void)
|
|
{
|
|
#if (MOTOR_OTA_CALIBRATION != 0u)
|
|
if(false != otaCalibrationDynamicActive)
|
|
{
|
|
OtaCalibration_StopDynamic(OTA_CALIBRATION_ERROR_NONE, true);
|
|
}
|
|
if(false != OtaCalibration_AnyInteractiveMoveActive())
|
|
{
|
|
OtaCalibration_FinalizeAllInteractiveMoves(
|
|
OTA_CALIBRATION_ERROR_NONE, true, true);
|
|
}
|
|
if((false != otaSeatJobActive) ||
|
|
(otaCalibrationPendingJob != OTA_SEAT_JOB_NONE))
|
|
{
|
|
OtaCalibration_RequestAbort(OTA_CALIBRATION_ERROR_ABORTED);
|
|
otaCalibrationPendingJob = OTA_SEAT_JOB_NONE;
|
|
}
|
|
#endif
|
|
#if (MOTOR_OTA_SELF_COMPOSE != 0u)
|
|
OtaSelf_Abort();
|
|
#endif
|
|
}
|
|
|
|
static void SeatDemoCompat_StopNativeOwnerForAxis(uint32_t axis)
|
|
{
|
|
if(false != SeatDemoDynamic_IsActive())
|
|
{
|
|
SeatDemoDynamic_Stop(seatDemoSequenceReference);
|
|
}
|
|
#if (MOTOR_OTA_CALIBRATION != 0u)
|
|
if(false != otaCalibrationDynamicActive)
|
|
{
|
|
OtaCalibration_StopDynamic(OTA_CALIBRATION_ERROR_NONE, true);
|
|
}
|
|
if((otaCalibrationMoveActiveMask & (1ul << axis)) != 0ul)
|
|
{
|
|
OtaCalibration_FinalizeInteractiveAxis(
|
|
axis, OTA_CALIBRATION_ERROR_NONE, true, true);
|
|
}
|
|
if((false != otaSeatJobActive) &&
|
|
(otaSeatJobType != OTA_SEAT_JOB_MOVE) &&
|
|
(otaSeatJobType != OTA_SEAT_JOB_DYNAMIC))
|
|
{
|
|
OtaCalibration_RequestAbort(OTA_CALIBRATION_ERROR_ABORTED);
|
|
otaCalibrationPendingJob = OTA_SEAT_JOB_NONE;
|
|
}
|
|
#else
|
|
(void)axis;
|
|
#endif
|
|
#if (MOTOR_OTA_SELF_COMPOSE != 0u)
|
|
OtaSelf_Abort();
|
|
#endif
|
|
}
|
|
|
|
static void SeatDemoCompat_HandleCommand(const ota_can_frame_t *frame)
|
|
{
|
|
seat_demo_command_t command = { 0u };
|
|
seat_demo_reject_t reject = SEAT_DEMO_REJECT_BAD_DLC;
|
|
uint32_t axis;
|
|
bool calibrationCommand;
|
|
|
|
if((false == SeatDemoCompat_ParseCommand(frame, &command, &reject)) ||
|
|
(false == SeatDemoCompat_ParametersValid(&command, &reject)))
|
|
{
|
|
SeatDemoCompat_SetResponse(
|
|
SEAT_DEMO_ACK_REJECTED, reject, &command);
|
|
return;
|
|
}
|
|
|
|
if(command.opcode != SEAT_DEMO_OPCODE_STOP_ALL)
|
|
{
|
|
if((false != seatDemoLastSequenceValid) &&
|
|
(command.sequence == seatDemoSequenceReference))
|
|
{
|
|
if((false != seatDemoLastCommandValid) &&
|
|
(false != SeatDemoCompat_CommandEqual(
|
|
&command, &seatDemoLastCommand)))
|
|
{
|
|
SeatDemoCompat_SetResponse(
|
|
SEAT_DEMO_ACK_DUPLICATE,
|
|
SEAT_DEMO_REJECT_NONE,
|
|
&command);
|
|
}
|
|
else
|
|
{
|
|
SeatDemoCompat_SetResponse(
|
|
SEAT_DEMO_ACK_REJECTED,
|
|
SEAT_DEMO_REJECT_SEQUENCE_OLD,
|
|
&command);
|
|
}
|
|
return;
|
|
}
|
|
if((false != seatDemoLastSequenceValid) &&
|
|
(false == SeatDemoCompat_SequenceNewer(
|
|
command.sequence, seatDemoSequenceReference)))
|
|
{
|
|
SeatDemoCompat_SetResponse(
|
|
SEAT_DEMO_ACK_REJECTED,
|
|
SEAT_DEMO_REJECT_SEQUENCE_OLD,
|
|
&command);
|
|
return;
|
|
}
|
|
}
|
|
|
|
seatDemoLastSequenceValid = true;
|
|
seatDemoLastCommandValid = true;
|
|
seatDemoSequenceReference = command.sequence;
|
|
seatDemoLastCommand = command;
|
|
|
|
if(command.opcode == SEAT_DEMO_OPCODE_STOP_ALL)
|
|
{
|
|
SeatDemoCompat_StopNativeOwnersForAll();
|
|
SeatDemoCompat_StopAllInternal(command.sequence);
|
|
SeatDemoCompat_SetResponse(
|
|
SEAT_DEMO_ACK_STOP_ALL_ACCEPTED,
|
|
SEAT_DEMO_REJECT_NONE,
|
|
&command);
|
|
return;
|
|
}
|
|
if(command.opcode == SEAT_DEMO_OPCODE_STOP_DYNAMIC_MODE)
|
|
{
|
|
SeatDemoDynamic_Stop(command.sequence);
|
|
SeatDemoCompat_SetResponse(
|
|
SEAT_DEMO_ACK_ACCEPTED,
|
|
SEAT_DEMO_REJECT_NONE,
|
|
&command);
|
|
return;
|
|
}
|
|
axis = command.motor;
|
|
if(command.opcode == SEAT_DEMO_OPCODE_STOP_MOTOR)
|
|
{
|
|
SeatDemoCompat_StopNativeOwnerForAxis(axis);
|
|
SeatDemoCompat_StopAxis(axis, command.sequence);
|
|
SeatDemoCompat_SetResponse(
|
|
SEAT_DEMO_ACK_ACCEPTED,
|
|
SEAT_DEMO_REJECT_NONE,
|
|
&command);
|
|
return;
|
|
}
|
|
if(command.opcode == SEAT_DEMO_OPCODE_QUERY_STATUS)
|
|
{
|
|
seatDemoMotorStatusPendingMask |= (uint8_t)(1u << axis);
|
|
seatDemoDiagnosticPendingMask |= (uint8_t)(1u << axis);
|
|
seatDemoDynamicStatusPending = true;
|
|
SeatDemoCompat_SetResponse(
|
|
SEAT_DEMO_ACK_ACCEPTED,
|
|
SEAT_DEMO_REJECT_NONE,
|
|
&command);
|
|
return;
|
|
}
|
|
if(command.opcode == SEAT_DEMO_OPCODE_START_DYNAMIC_MODE)
|
|
{
|
|
if(false != driverFaultLatched)
|
|
{
|
|
SeatDemoCompat_SetResponse(
|
|
SEAT_DEMO_ACK_REJECTED,
|
|
SEAT_DEMO_REJECT_FAULT_ACTIVE,
|
|
&command);
|
|
return;
|
|
}
|
|
#if (MOTOR_OTA_CALIBRATION != 0u)
|
|
if((otaCalibrationPendingJob != OTA_SEAT_JOB_NONE) ||
|
|
((false != otaSeatJobActive) &&
|
|
(otaSeatJobType != OTA_SEAT_JOB_MOVE) &&
|
|
(otaSeatJobType != OTA_SEAT_JOB_DYNAMIC)))
|
|
{
|
|
SeatDemoCompat_SetResponse(
|
|
SEAT_DEMO_ACK_REJECTED,
|
|
SEAT_DEMO_REJECT_BUSY_SAME_MOTOR,
|
|
&command);
|
|
return;
|
|
}
|
|
#endif
|
|
if((SeatDemoCompat_ActiveMask() != 0u) ||
|
|
(false != SeatDemoCompat_CalibrationBusy()))
|
|
{
|
|
SeatDemoCompat_SetResponse(
|
|
SEAT_DEMO_ACK_REJECTED,
|
|
SEAT_DEMO_REJECT_BUSY_SAME_MOTOR,
|
|
&command);
|
|
return;
|
|
}
|
|
if(SeatDemoCompat_PositionValidMask() != SEAT_DEMO_ACTIVE_MASK)
|
|
{
|
|
SeatDemoCompat_SetResponse(
|
|
SEAT_DEMO_ACK_REJECTED,
|
|
SEAT_DEMO_REJECT_POSITION_INVALID,
|
|
&command);
|
|
return;
|
|
}
|
|
SeatDemoCompat_StopNativeOwnersForAll();
|
|
if(false == SeatDemoDynamic_Start(
|
|
(uint8_t)command.paramA, command.sequence))
|
|
{
|
|
SeatDemoCompat_SetResponse(
|
|
SEAT_DEMO_ACK_REJECTED,
|
|
SEAT_DEMO_REJECT_INTERNAL_ERROR,
|
|
&command);
|
|
return;
|
|
}
|
|
SeatDemoCompat_SetResponse(
|
|
SEAT_DEMO_ACK_ACCEPTED,
|
|
SEAT_DEMO_REJECT_NONE,
|
|
&command);
|
|
return;
|
|
}
|
|
if(command.opcode == SEAT_DEMO_OPCODE_CLEAR_FAULT)
|
|
{
|
|
if((false != seatDemoTasks[axis].active) ||
|
|
(false != seatDemoTasks[axis].pending))
|
|
{
|
|
SeatDemoCompat_SetResponse(
|
|
SEAT_DEMO_ACK_REJECTED,
|
|
SEAT_DEMO_REJECT_BUSY_SAME_MOTOR,
|
|
&command);
|
|
return;
|
|
}
|
|
if(false != driverFaultLatched)
|
|
{
|
|
SeatDemoCompat_SetResponse(
|
|
SEAT_DEMO_ACK_REJECTED,
|
|
SEAT_DEMO_REJECT_FAULT_ACTIVE,
|
|
&command);
|
|
return;
|
|
}
|
|
seatDemoTasks[axis].state = SEAT_DEMO_TASK_IDLE;
|
|
seatDemoTasks[axis].result = SEAT_DEMO_RESULT_NONE;
|
|
seatDemoMotorStatusPendingMask |= (uint8_t)(1u << axis);
|
|
seatDemoDiagnosticPendingMask |= (uint8_t)(1u << axis);
|
|
SeatDemoCompat_SetResponse(
|
|
SEAT_DEMO_ACK_ACCEPTED,
|
|
SEAT_DEMO_REJECT_NONE,
|
|
&command);
|
|
return;
|
|
}
|
|
|
|
if(false != driverFaultLatched)
|
|
{
|
|
SeatDemoCompat_SetResponse(
|
|
SEAT_DEMO_ACK_REJECTED,
|
|
SEAT_DEMO_REJECT_FAULT_ACTIVE,
|
|
&command);
|
|
return;
|
|
}
|
|
if((false != SeatDemoCompat_IsPositionOpcode(command.opcode)) &&
|
|
(false == SeatDemoCompat_PositionValid(axis)))
|
|
{
|
|
SeatDemoCompat_SetResponse(
|
|
SEAT_DEMO_ACK_REJECTED,
|
|
SEAT_DEMO_REJECT_POSITION_INVALID,
|
|
&command);
|
|
return;
|
|
}
|
|
if(false != SeatDemoCompat_CalibrationBusy())
|
|
{
|
|
SeatDemoCompat_SetResponse(
|
|
SEAT_DEMO_ACK_REJECTED,
|
|
SEAT_DEMO_REJECT_BUSY_SAME_MOTOR,
|
|
&command);
|
|
return;
|
|
}
|
|
|
|
calibrationCommand =
|
|
(command.opcode == SEAT_DEMO_OPCODE_CALIBRATE_RANGE) ||
|
|
(command.opcode == SEAT_DEMO_OPCODE_HOME_KNOWN_RANGE);
|
|
if(false != calibrationCommand)
|
|
{
|
|
if((SeatDemoCompat_ActiveMask() != 0u) ||
|
|
(false != SeatDemoCompat_NativeJobBlocksMotion(axis, true)))
|
|
{
|
|
SeatDemoCompat_SetResponse(
|
|
SEAT_DEMO_ACK_REJECTED,
|
|
SEAT_DEMO_REJECT_BUSY_SAME_MOTOR,
|
|
&command);
|
|
return;
|
|
}
|
|
}
|
|
else if((false != seatDemoTasks[axis].active) ||
|
|
(false != seatDemoTasks[axis].pending))
|
|
{
|
|
SeatDemoCompat_StopAxis(axis, command.sequence);
|
|
}
|
|
else if(SeatDemoCompat_ActiveCount() >= SEAT_DEMO_MOTOR_COUNT)
|
|
{
|
|
SeatDemoCompat_SetResponse(
|
|
SEAT_DEMO_ACK_REJECTED,
|
|
SEAT_DEMO_REJECT_INTERLOCK,
|
|
&command);
|
|
return;
|
|
}
|
|
|
|
SeatDemoCompat_QueueCommand(&command);
|
|
SeatDemoCompat_SetResponse(
|
|
SEAT_DEMO_ACK_ACCEPTED,
|
|
SEAT_DEMO_REJECT_NONE,
|
|
&command);
|
|
}
|
|
|
|
static void SeatDemoCompat_ServiceCalibrationTimeout(void)
|
|
{
|
|
uint32_t axis = seatDemoCalibrationAxis;
|
|
seat_demo_motor_task_t *task;
|
|
|
|
if((false == seatDemoCalibrationWorker) ||
|
|
(axis >= SEAT_DEMO_MOTOR_COUNT))
|
|
{
|
|
return;
|
|
}
|
|
task = &seatDemoTasks[axis];
|
|
if((false != task->active) &&
|
|
(false != SeatDemoCompat_TimeReached(
|
|
hallTimebaseTicks, task->deadlineTicks)))
|
|
{
|
|
(void)Motor_StopMode((motor_mode_t)axis);
|
|
task->active = false;
|
|
task->state = SEAT_DEMO_TASK_FAILED;
|
|
task->result = SEAT_DEMO_RESULT_CALIBRATION_FAILED;
|
|
seatDemoMotorStatusPendingMask |= (uint8_t)(1u << axis);
|
|
seatDemoDiagnosticPendingMask |= (uint8_t)(1u << axis);
|
|
seatDemoSystemStatusPending = true;
|
|
}
|
|
}
|
|
|
|
static void SeatDemoCompat_ServiceTasks(void)
|
|
{
|
|
uint32_t axis;
|
|
uint32_t now = hallTimebaseTicks;
|
|
|
|
if(false != seatDemoCalibrationWorker)
|
|
{
|
|
SeatDemoCompat_ServiceCalibrationTimeout();
|
|
return;
|
|
}
|
|
|
|
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
|
{
|
|
seat_demo_motor_task_t *task = &seatDemoTasks[axis];
|
|
motor_event_t event;
|
|
motor_direction_t direction;
|
|
bool settled;
|
|
uint32_t rawEdges;
|
|
uint32_t interruptState;
|
|
|
|
if(false == task->active)
|
|
{
|
|
continue;
|
|
}
|
|
interruptState = Cy_SysLib_EnterCriticalSection();
|
|
event = motorRuntime[axis].lastEvent;
|
|
direction = motorRuntime[axis].direction;
|
|
settled = (direction == MOTOR_DIRECTION_STOPPED) &&
|
|
(false == motorRuntime[axis].startPending) &&
|
|
(false == motorRuntime[axis].targetActive) &&
|
|
(false == motorRuntime[axis].coastTracking);
|
|
rawEdges = motorRuntime[axis].rawRisingEdges;
|
|
Cy_SysLib_ExitCriticalSection(interruptState);
|
|
|
|
if((false != driverFaultLatched) ||
|
|
(event == MOTOR_EVENT_DRIVER_FAULT))
|
|
{
|
|
SeatDemoCompat_SetTaskFailed(
|
|
axis, SEAT_DEMO_RESULT_DRIVER_FAULT);
|
|
continue;
|
|
}
|
|
if(event == MOTOR_EVENT_ABORTED)
|
|
{
|
|
SeatDemoCompat_MarkTaskStopped(
|
|
axis, task->command.sequence);
|
|
continue;
|
|
}
|
|
|
|
if((task->state == SEAT_DEMO_TASK_WAIT_START) &&
|
|
(direction != MOTOR_DIRECTION_STOPPED))
|
|
{
|
|
task->state =
|
|
(task->command.opcode == SEAT_DEMO_OPCODE_MOVE_TIMED) ?
|
|
SEAT_DEMO_TASK_TIMED : SEAT_DEMO_TASK_POSITION;
|
|
task->deadlineTicks = now + task->durationTicks;
|
|
seatDemoMotorStatusPendingMask |= (uint8_t)(1u << axis);
|
|
}
|
|
if(task->state == SEAT_DEMO_TASK_RETRY_DELAY)
|
|
{
|
|
if(false != SeatDemoCompat_TimeReached(now, task->retryAtTicks))
|
|
{
|
|
motor_safety_options_t safety = { false, false };
|
|
|
|
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, task->dutyPercent)) ||
|
|
(MOTOR_API_SUCCESS != Motor_StartMoveToValue(
|
|
(motor_mode_t)axis, task->targetValueX100)))
|
|
{
|
|
SeatDemoCompat_SetTaskFailed(
|
|
axis, SEAT_DEMO_RESULT_STALL);
|
|
}
|
|
else
|
|
{
|
|
task->state = SEAT_DEMO_TASK_WAIT_START;
|
|
task->deadlineTicks = 0ul;
|
|
}
|
|
}
|
|
continue;
|
|
}
|
|
|
|
if((event == MOTOR_EVENT_STALL) ||
|
|
(event == MOTOR_EVENT_NEAR_ENDPOINT) ||
|
|
(event == MOTOR_EVENT_FAR_ENDPOINT))
|
|
{
|
|
bool endpoint = (event != MOTOR_EVENT_STALL);
|
|
if((task->command.opcode != SEAT_DEMO_OPCODE_MOVE_TIMED) &&
|
|
(false != SeatDemoCompat_TargetWithinTolerance(axis)))
|
|
{
|
|
SeatDemoCompat_SetTaskComplete(axis);
|
|
}
|
|
else if((task->command.opcode != SEAT_DEMO_OPCODE_MOVE_TIMED) &&
|
|
(task->retryCount == 0u))
|
|
{
|
|
(void)Motor_StopMode((motor_mode_t)axis);
|
|
task->state = SEAT_DEMO_TASK_RETRY_DELAY;
|
|
task->deadlineTicks = 0ul;
|
|
task->retryAtTicks = now +
|
|
MOTOR_MS_TO_TICKS(SEAT_DEMO_RETRY_DELAY_MS);
|
|
task->retryCount = 1u;
|
|
seatDemoMotorStatusPendingMask |= (uint8_t)(1u << axis);
|
|
}
|
|
else if((false != endpoint) &&
|
|
(task->command.opcode == SEAT_DEMO_OPCODE_MOVE_TIMED))
|
|
{
|
|
SeatDemoCompat_SetTaskComplete(axis);
|
|
}
|
|
else
|
|
{
|
|
SeatDemoCompat_SetTaskFailed(
|
|
axis, (false != endpoint) ?
|
|
SEAT_DEMO_RESULT_ENDPOINT : SEAT_DEMO_RESULT_STALL);
|
|
}
|
|
continue;
|
|
}
|
|
if(event == MOTOR_EVENT_TARGET_REACHED)
|
|
{
|
|
SeatDemoCompat_SetTaskComplete(axis);
|
|
continue;
|
|
}
|
|
if(event == MOTOR_EVENT_TIMEOUT)
|
|
{
|
|
SeatDemoCompat_SetTaskFailed(axis, SEAT_DEMO_RESULT_TIMEOUT);
|
|
continue;
|
|
}
|
|
if((task->deadlineTicks != 0ul) &&
|
|
(false != SeatDemoCompat_TimeReached(now, task->deadlineTicks)))
|
|
{
|
|
(void)Motor_StopMode((motor_mode_t)axis);
|
|
if(task->state == SEAT_DEMO_TASK_TIMED)
|
|
{
|
|
if(rawEdges == task->rawStartEdges)
|
|
{
|
|
SeatDemoCompat_SetTaskFailed(
|
|
axis, SEAT_DEMO_RESULT_STALL);
|
|
}
|
|
else
|
|
{
|
|
SeatDemoCompat_SetTaskComplete(axis);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
SeatDemoCompat_SetTaskFailed(
|
|
axis, SEAT_DEMO_RESULT_TIMEOUT);
|
|
}
|
|
continue;
|
|
}
|
|
if((task->state == SEAT_DEMO_TASK_WAIT_START) &&
|
|
(false != settled) &&
|
|
(event == MOTOR_EVENT_TARGET_REACHED))
|
|
{
|
|
SeatDemoCompat_SetTaskComplete(axis);
|
|
}
|
|
}
|
|
}
|
|
|
|
static uint8_t SeatDemoCompat_MapTaskState(
|
|
const seat_demo_motor_task_t *task)
|
|
{
|
|
switch(task->state)
|
|
{
|
|
case SEAT_DEMO_TASK_IDLE:
|
|
return 0u;
|
|
case SEAT_DEMO_TASK_WAIT_START:
|
|
case SEAT_DEMO_TASK_RETRY_DELAY:
|
|
return 1u;
|
|
case SEAT_DEMO_TASK_POSITION:
|
|
return 2u;
|
|
case SEAT_DEMO_TASK_TIMED:
|
|
return 3u;
|
|
case SEAT_DEMO_TASK_CALIBRATION:
|
|
return 4u;
|
|
case SEAT_DEMO_TASK_COMPLETE:
|
|
return 6u;
|
|
case SEAT_DEMO_TASK_FAILED:
|
|
default:
|
|
return 7u;
|
|
}
|
|
}
|
|
|
|
static uint8_t SeatDemoCompat_MapTaskResult(
|
|
const seat_demo_motor_task_t *task)
|
|
{
|
|
switch(task->result)
|
|
{
|
|
case SEAT_DEMO_RESULT_NONE:
|
|
return 0u;
|
|
case SEAT_DEMO_RESULT_RUNNING:
|
|
return 1u;
|
|
case SEAT_DEMO_RESULT_OK:
|
|
return 2u;
|
|
case SEAT_DEMO_RESULT_STOPPED:
|
|
return 3u;
|
|
case SEAT_DEMO_RESULT_ENDPOINT:
|
|
return 4u;
|
|
case SEAT_DEMO_RESULT_STALL:
|
|
return 5u;
|
|
case SEAT_DEMO_RESULT_TIMEOUT:
|
|
return 6u;
|
|
case SEAT_DEMO_RESULT_DRIVER_FAULT:
|
|
return 7u;
|
|
case SEAT_DEMO_RESULT_INVALID:
|
|
return 9u;
|
|
case SEAT_DEMO_RESULT_CALIBRATION_FAILED:
|
|
default:
|
|
return 11u;
|
|
}
|
|
}
|
|
|
|
static uint8_t SeatDemoCompat_FaultedMask(void)
|
|
{
|
|
uint32_t axis;
|
|
uint8_t mask = SeatDemoDynamic_FaultMask();
|
|
|
|
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
|
{
|
|
seat_demo_result_t result = seatDemoTasks[axis].result;
|
|
if((result == SEAT_DEMO_RESULT_STALL) ||
|
|
(result == SEAT_DEMO_RESULT_DRIVER_FAULT) ||
|
|
(result == SEAT_DEMO_RESULT_INVALID) ||
|
|
(result == SEAT_DEMO_RESULT_CALIBRATION_FAILED))
|
|
{
|
|
mask |= (uint8_t)(1u << axis);
|
|
}
|
|
}
|
|
if(false != driverFaultLatched)
|
|
{
|
|
mask |= SEAT_DEMO_ACTIVE_MASK;
|
|
}
|
|
return mask;
|
|
}
|
|
|
|
static uint8_t SeatDemoCompat_PositionValidMask(void)
|
|
{
|
|
uint32_t axis;
|
|
uint8_t mask = 0u;
|
|
uint32_t interruptState = Cy_SysLib_EnterCriticalSection();
|
|
|
|
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
|
{
|
|
if(false != motorRuntime[axis].positionValid)
|
|
{
|
|
mask |= (uint8_t)(1u << axis);
|
|
}
|
|
}
|
|
Cy_SysLib_ExitCriticalSection(interruptState);
|
|
return mask;
|
|
}
|
|
|
|
static uint8_t SeatDemoCompat_SystemState(void)
|
|
{
|
|
if(false != driverFaultLatched)
|
|
{
|
|
return SEAT_DEMO_SYSTEM_FAULT;
|
|
}
|
|
if(false != SeatDemoCompat_CalibrationBusy())
|
|
{
|
|
return SEAT_DEMO_SYSTEM_CALIBRATION;
|
|
}
|
|
if(SeatDemoCompat_FaultedMask() != 0u)
|
|
{
|
|
return SEAT_DEMO_SYSTEM_FAULT;
|
|
}
|
|
if(SeatDemoCompat_ActiveMask() != 0u)
|
|
{
|
|
return SEAT_DEMO_SYSTEM_ACTIVE;
|
|
}
|
|
return SEAT_DEMO_SYSTEM_READY;
|
|
}
|
|
|
|
static bool SeatDemoCompat_SendSystemStatus(void)
|
|
{
|
|
uint8_t data[8];
|
|
uint8_t activeMask = SeatDemoCompat_ActiveMask();
|
|
uint8_t faultedMask = SeatDemoCompat_FaultedMask();
|
|
uint8_t validMask = SeatDemoCompat_PositionValidMask();
|
|
|
|
data[0] = (uint8_t)((seatDemoAck & 0x0Fu) |
|
|
((SeatDemoCompat_SystemState() & 0x0Fu) << 4u));
|
|
data[1] = seatDemoReject;
|
|
SeatDemoCompat_WriteLe16(&data[2], seatDemoLastSequence);
|
|
data[4] = seatDemoLastOpcode;
|
|
data[5] = (uint8_t)((activeMask & 0x0Fu) |
|
|
((false != SeatDemoCompat_CalibrationBusy()) ? 0x40u : 0u) |
|
|
(((faultedMask != 0u) ||
|
|
(false != driverFaultLatched)) ? 0x80u : 0u));
|
|
data[6] = (uint8_t)((faultedMask & 0x0Fu) |
|
|
((validMask & 0x0Fu) << 4u));
|
|
data[7] = seatDemoSystemAlive & 0x0Fu;
|
|
return OtaCan_QueueFrame(SEAT_DEMO_CAN_SYSTEM_STATUS_ID, data);
|
|
}
|
|
|
|
static bool SeatDemoCompat_SendMotorStatus(uint32_t axis)
|
|
{
|
|
seat_demo_motor_task_t *task = &seatDemoTasks[axis];
|
|
uint8_t data[8];
|
|
uint32_t interruptState;
|
|
uint32_t position;
|
|
uint32_t range;
|
|
motor_direction_t direction;
|
|
motor_event_t event;
|
|
bool valid;
|
|
bool moving;
|
|
uint8_t duty;
|
|
uint16_t normalized;
|
|
uint8_t flags;
|
|
|
|
interruptState = Cy_SysLib_EnterCriticalSection();
|
|
position = motorRuntime[axis].positionHallCount;
|
|
range = motorRuntime[axis].maxHallCount;
|
|
direction = motorRuntime[axis].direction;
|
|
event = motorRuntime[axis].lastEvent;
|
|
valid = motorRuntime[axis].positionValid;
|
|
moving = (direction != MOTOR_DIRECTION_STOPPED) ||
|
|
motorRuntime[axis].startPending;
|
|
duty = motorRuntime[axis].speedControlEnabled ?
|
|
motorRuntime[axis].speedCurrentDutyPercent :
|
|
motorRuntime[axis].fixedDutyPercent;
|
|
Cy_SysLib_ExitCriticalSection(interruptState);
|
|
|
|
if(false != task->pending)
|
|
{
|
|
direction = task->command.direction;
|
|
duty = SeatDemoCompat_CommandDuty(&task->command);
|
|
}
|
|
flags = (uint8_t)(((uint8_t)direction & 0x03u) |
|
|
(((false != task->active) ||
|
|
(false != task->pending) ||
|
|
((false != SeatDemoDynamic_IsActive()) &&
|
|
(false != moving))) ? 0x04u : 0u) |
|
|
((false != valid) ? 0x08u : 0u) |
|
|
(((false != valid) && (position == 0ul)) ? 0x10u : 0u) |
|
|
(((false != valid) && (range != 0ul) &&
|
|
(position >= range)) ? 0x20u : 0u) |
|
|
((task->result == SEAT_DEMO_RESULT_STALL) ? 0x40u : 0u) |
|
|
(((task->result == SEAT_DEMO_RESULT_DRIVER_FAULT) ||
|
|
(false != driverFaultLatched)) ? 0x80u : 0u));
|
|
(void)event;
|
|
(void)moving;
|
|
normalized = ((false != valid) && (range != 0ul)) ?
|
|
(uint16_t)((((position > range) ? range : position) *
|
|
10000ul) / range) : 0xFFFFu;
|
|
|
|
data[0] = (uint8_t)axis;
|
|
data[1] = (uint8_t)((SeatDemoCompat_MapTaskState(task) & 0x0Fu) |
|
|
((SeatDemoCompat_MapTaskResult(task) & 0x0Fu) << 4u));
|
|
data[2] = flags;
|
|
data[3] = (uint8_t)task->command.sequence;
|
|
data[4] = (uint8_t)(task->command.sequence >> 8u);
|
|
SeatDemoCompat_WriteLe16(&data[5], normalized);
|
|
data[7] = ((false != task->active) ||
|
|
(false != task->pending)) ? duty : 0u;
|
|
return OtaCan_QueueFrame(SEAT_DEMO_CAN_MOTOR_STATUS_ID, data);
|
|
}
|
|
|
|
static bool SeatDemoCompat_SendCurrentDiagnostic(uint32_t axis)
|
|
{
|
|
uint8_t data[8];
|
|
uint8_t flags;
|
|
|
|
flags = (uint8_t)
|
|
((seatDemoCurrentSampleValid[axis] ? 0x01u : 0u) |
|
|
(seatDemoCurrentBaselineValid[axis] ? 0x02u : 0u) |
|
|
(seatDemoCurrentHigh[axis] ? 0x04u : 0u) |
|
|
((motorRuntime[axis].recentWindowPulses <=
|
|
MOTOR_STALL_MAX_PULSES) ? 0x08u : 0u) |
|
|
(motorRuntime[axis].calibrating ? 0x10u : 0u));
|
|
data[0] = (uint8_t)axis;
|
|
data[1] = flags;
|
|
SeatDemoCompat_WriteLe16(&data[2], seatDemoCurrentRaw[axis]);
|
|
SeatDemoCompat_WriteLe16(&data[4], seatDemoCurrentBaseline[axis]);
|
|
SeatDemoCompat_WriteLe16(&data[6], seatDemoCurrentThreshold[axis]);
|
|
return OtaCan_QueueFrame(
|
|
SEAT_DEMO_CAN_CURRENT_DIAGNOSTIC_ID, data);
|
|
}
|
|
|
|
static bool SeatDemoCompat_SendDiagnostic(uint32_t axis)
|
|
{
|
|
seat_demo_motor_task_t *task = &seatDemoTasks[axis];
|
|
uint8_t data[8];
|
|
uint32_t interruptState;
|
|
uint32_t position;
|
|
uint32_t range;
|
|
uint32_t recentPulses;
|
|
motor_event_t event;
|
|
uint8_t faultFlags;
|
|
uint32_t speedPpsX100;
|
|
uint32_t speedRaw;
|
|
|
|
interruptState = Cy_SysLib_EnterCriticalSection();
|
|
position = motorRuntime[axis].positionHallCount;
|
|
range = motorRuntime[axis].maxHallCount;
|
|
recentPulses = motorRuntime[axis].recentWindowPulses;
|
|
event = motorRuntime[axis].lastEvent;
|
|
Cy_SysLib_ExitCriticalSection(interruptState);
|
|
if(position > 0xFFFFul) position = 0xFFFFul;
|
|
if(range > 0xFFFFul) range = 0xFFFFul;
|
|
speedPpsX100 = (recentPulses * 100000ul) /
|
|
MOTOR_SAFETY_WINDOW_MS;
|
|
speedRaw = (speedPpsX100 + 100ul) / 200ul;
|
|
if(speedRaw > 0xFFul) speedRaw = 0xFFul;
|
|
faultFlags = (uint8_t)
|
|
((task->result == SEAT_DEMO_RESULT_STALL ? 0x01u : 0u) |
|
|
(((task->result == SEAT_DEMO_RESULT_DRIVER_FAULT) ||
|
|
(false != driverFaultLatched)) ? 0x02u : 0u) |
|
|
(task->result == SEAT_DEMO_RESULT_INVALID ? 0x04u : 0u) |
|
|
(task->result == SEAT_DEMO_RESULT_CALIBRATION_FAILED ?
|
|
0x08u : 0u));
|
|
|
|
data[0] = (uint8_t)axis;
|
|
data[1] = (uint8_t)(((uint8_t)event & 0x0Fu) |
|
|
((faultFlags & 0x0Fu) << 4u));
|
|
SeatDemoCompat_WriteLe16(&data[2], (uint16_t)position);
|
|
SeatDemoCompat_WriteLe16(&data[4], (uint16_t)range);
|
|
data[6] = (uint8_t)speedRaw;
|
|
data[7] = (uint8_t)((task->retryCount & 0x0Fu) |
|
|
((seatDemoDiagnosticAlive[axis] & 0x0Fu) << 4u));
|
|
if(false == OtaCan_QueueFrame(SEAT_DEMO_CAN_DIAGNOSTIC_ID, data))
|
|
{
|
|
return false;
|
|
}
|
|
return SeatDemoCompat_SendCurrentDiagnostic(axis);
|
|
}
|
|
|
|
static void SeatDemoCompat_ServiceFeedback(void)
|
|
{
|
|
uint32_t now = hallTimebaseTicks;
|
|
uint32_t axis;
|
|
|
|
if(false != SeatDemoCompat_TimeReached(
|
|
now, seatDemoNextSystemStatusTicks))
|
|
{
|
|
seatDemoSystemStatusPending = true;
|
|
seatDemoNextSystemStatusTicks = now +
|
|
MOTOR_MS_TO_TICKS(SEAT_DEMO_SYSTEM_STATUS_MS);
|
|
}
|
|
if(false != SeatDemoCompat_TimeReached(
|
|
now, seatDemoDynamicNextStatusTicks))
|
|
{
|
|
seatDemoDynamicStatusPending = true;
|
|
seatDemoDynamicNextStatusTicks = now +
|
|
MOTOR_MS_TO_TICKS(SEAT_DEMO_DYNAMIC_STATUS_MS);
|
|
}
|
|
if((false == seatDemoMotorPeriodicPending) &&
|
|
(false != SeatDemoCompat_TimeReached(
|
|
now, seatDemoNextMotorPollTicks)))
|
|
{
|
|
seatDemoMotorPeriodicId = seatDemoMotorPollCursor;
|
|
seatDemoMotorStatusPendingMask |=
|
|
(uint8_t)(1u << seatDemoMotorPeriodicId);
|
|
seatDemoMotorPeriodicPending = true;
|
|
}
|
|
if((false == seatDemoDiagnosticPeriodicPending) &&
|
|
(false != SeatDemoCompat_TimeReached(
|
|
now, seatDemoNextDiagnosticPollTicks)))
|
|
{
|
|
seatDemoDiagnosticPeriodicId = seatDemoDiagnosticPollCursor;
|
|
seatDemoDiagnosticPendingMask |=
|
|
(uint8_t)(1u << seatDemoDiagnosticPeriodicId);
|
|
seatDemoDiagnosticPeriodicPending = true;
|
|
}
|
|
|
|
if(false != seatDemoSystemStatusPending)
|
|
{
|
|
if(false != SeatDemoCompat_SendSystemStatus())
|
|
{
|
|
seatDemoSystemStatusPending = false;
|
|
seatDemoSystemAlive = (uint8_t)
|
|
((seatDemoSystemAlive + 1u) & 0x0Fu);
|
|
if(false != seatDemoAckNeedsClear)
|
|
{
|
|
seatDemoAck = SEAT_DEMO_ACK_NONE;
|
|
seatDemoReject = SEAT_DEMO_REJECT_NONE;
|
|
seatDemoAckNeedsClear = false;
|
|
}
|
|
}
|
|
return;
|
|
}
|
|
if(false != seatDemoDynamicStatusPending)
|
|
{
|
|
if(false != SeatDemoDynamic_SendStatus())
|
|
{
|
|
seatDemoDynamicStatusPending = false;
|
|
}
|
|
return;
|
|
}
|
|
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
|
{
|
|
uint8_t bit = (uint8_t)(1u << axis);
|
|
if((seatDemoMotorStatusPendingMask & bit) != 0u)
|
|
{
|
|
if(false != SeatDemoCompat_SendMotorStatus(axis))
|
|
{
|
|
seatDemoMotorStatusPendingMask &= (uint8_t)(~bit);
|
|
if((false != seatDemoMotorPeriodicPending) &&
|
|
(axis == seatDemoMotorPeriodicId))
|
|
{
|
|
seatDemoMotorPollCursor = (uint8_t)
|
|
((seatDemoMotorPollCursor + 1u) %
|
|
SEAT_DEMO_MOTOR_COUNT);
|
|
seatDemoMotorPeriodicPending = false;
|
|
seatDemoNextMotorPollTicks = now +
|
|
MOTOR_MS_TO_TICKS(SEAT_DEMO_MOTOR_POLL_MS);
|
|
}
|
|
}
|
|
return;
|
|
}
|
|
}
|
|
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
|
{
|
|
uint8_t bit = (uint8_t)(1u << axis);
|
|
if((seatDemoDiagnosticPendingMask & bit) != 0u)
|
|
{
|
|
if(false != SeatDemoCompat_SendDiagnostic(axis))
|
|
{
|
|
seatDemoDiagnosticPendingMask &= (uint8_t)(~bit);
|
|
seatDemoDiagnosticAlive[axis] = (uint8_t)
|
|
((seatDemoDiagnosticAlive[axis] + 1u) & 0x0Fu);
|
|
if((false != seatDemoDiagnosticPeriodicPending) &&
|
|
(axis == seatDemoDiagnosticPeriodicId))
|
|
{
|
|
seatDemoDiagnosticPollCursor = (uint8_t)
|
|
((seatDemoDiagnosticPollCursor + 1u) %
|
|
SEAT_DEMO_MOTOR_COUNT);
|
|
seatDemoDiagnosticPeriodicPending = false;
|
|
seatDemoNextDiagnosticPollTicks = now +
|
|
MOTOR_MS_TO_TICKS(SEAT_DEMO_DIAGNOSTIC_POLL_MS);
|
|
}
|
|
}
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
|
|
static bool SeatDemoThermal_Init(void);
|
|
static void SeatDemoThermal_HandleFrame(const ota_can_frame_t *frame);
|
|
static void SeatDemoThermal_Service(void);
|
|
static void SeatDemoThermal_ForceSafeOff(void);
|
|
|
|
static bool SeatDemoCompat_IsValidStopAllFrame(
|
|
const ota_can_frame_t *frame)
|
|
{
|
|
uint8_t control;
|
|
|
|
if((NULL == frame) ||
|
|
(frame->identifier != SEAT_DEMO_CAN_COMMAND_ID) ||
|
|
(frame->dlc != 8u) ||
|
|
(frame->data[0] != SEAT_DEMO_OPCODE_STOP_ALL))
|
|
{
|
|
return false;
|
|
}
|
|
control = frame->data[1];
|
|
return ((control & 0x0Fu) == 0u) &&
|
|
(((control >> 4u) & 0x03u) ==
|
|
SEAT_DEMO_CAN_PROTOCOL_VERSION) &&
|
|
((control & 0xC0u) == 0u) &&
|
|
(SeatDemoCompat_ReadLe32(&frame->data[4]) == 0ul);
|
|
}
|
|
|
|
static void SeatDemoCompat_RxIsrHook(const ota_can_frame_t *frame)
|
|
{
|
|
uint32_t axis;
|
|
|
|
if(false == SeatDemoCompat_IsValidStopAllFrame(frame))
|
|
{
|
|
return;
|
|
}
|
|
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
|
{
|
|
Cy_Tcpwm_TriggerStopOrKill(motorProfiles[axis].pwmTcpwm);
|
|
Motor_SetPwmInputLow(&motorProfiles[axis]);
|
|
}
|
|
seatDemoStopAllSequenceFromIsr =
|
|
(uint16_t)((uint16_t)frame->data[2] |
|
|
((uint16_t)frame->data[3] << 8u));
|
|
seatDemoStopAllFromIsr = true;
|
|
}
|
|
|
|
void SeatDemoCompat_Init(void)
|
|
{
|
|
uint32_t axis;
|
|
|
|
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
|
{
|
|
seatDemoTasks[axis].command.opcode = SEAT_DEMO_OPCODE_INVALID;
|
|
seatDemoTasks[axis].command.motor = (uint8_t)axis;
|
|
seatDemoTasks[axis].command.direction = MOTOR_DIRECTION_STOPPED;
|
|
seatDemoTasks[axis].command.sequence = 0u;
|
|
seatDemoTasks[axis].command.paramA = 0u;
|
|
seatDemoTasks[axis].command.paramB = 0u;
|
|
seatDemoTasks[axis].command.dutyPercent = 0u;
|
|
seatDemoTasks[axis].command.explicitDuty = false;
|
|
seatDemoTasks[axis].state = SEAT_DEMO_TASK_IDLE;
|
|
seatDemoTasks[axis].result = SEAT_DEMO_RESULT_NONE;
|
|
seatDemoTasks[axis].pending = false;
|
|
seatDemoTasks[axis].active = false;
|
|
seatDemoDiagnosticAlive[axis] = 0u;
|
|
seatDemoCurrentBaselineValid[axis] = false;
|
|
seatDemoCurrentHigh[axis] = false;
|
|
seatDemoCurrentBaseline[axis] = 0u;
|
|
seatDemoCurrentThreshold[axis] = SEAT_DEMO_CURRENT_THRESHOLD_RAW;
|
|
}
|
|
for(axis = 0ul; axis < 6ul; axis++)
|
|
{
|
|
seatDemoCurrentSampleValid[axis] = false;
|
|
seatDemoCurrentRaw[axis] = 0u;
|
|
}
|
|
seatDemoLastSequenceValid = false;
|
|
seatDemoLastCommandValid = false;
|
|
seatDemoAckNeedsClear = false;
|
|
seatDemoCalibrationWorker = false;
|
|
seatDemoCalibrationAxis = 0xFFu;
|
|
seatDemoServiceDepth = 0u;
|
|
seatDemoAck = SEAT_DEMO_ACK_NONE;
|
|
seatDemoReject = SEAT_DEMO_REJECT_NONE;
|
|
seatDemoLastOpcode = SEAT_DEMO_OPCODE_INVALID;
|
|
seatDemoLastSequence = 0u;
|
|
seatDemoSequenceReference = 0u;
|
|
seatDemoPendingMask = 0u;
|
|
seatDemoMotorStatusPendingMask = SEAT_DEMO_ACTIVE_MASK;
|
|
seatDemoDiagnosticPendingMask = SEAT_DEMO_ACTIVE_MASK;
|
|
seatDemoDispatchCursor = 0u;
|
|
seatDemoMotorPollCursor = 0u;
|
|
seatDemoDiagnosticPollCursor = 0u;
|
|
seatDemoMotorPeriodicPending = false;
|
|
seatDemoDiagnosticPeriodicPending = false;
|
|
seatDemoSystemStatusPending = true;
|
|
seatDemoSystemAlive = 0u;
|
|
seatDemoNextStartTicks = hallTimebaseTicks;
|
|
seatDemoNextSystemStatusTicks = hallTimebaseTicks;
|
|
seatDemoNextMotorPollTicks = hallTimebaseTicks;
|
|
seatDemoNextDiagnosticPollTicks = hallTimebaseTicks;
|
|
seatDemoStopAllFromIsr = false;
|
|
seatDemoStopAllSequenceFromIsr = 0u;
|
|
SeatDemoDynamic_Init();
|
|
(void)SeatDemoThermal_Init();
|
|
OtaCan_SetRxIsrHook(SeatDemoCompat_RxIsrHook);
|
|
seatDemoInitialized = true;
|
|
}
|
|
|
|
void SeatDemoCompat_Service(void)
|
|
{
|
|
ota_can_frame_t frame;
|
|
|
|
if(false == seatDemoInitialized)
|
|
{
|
|
return;
|
|
}
|
|
if((seatDemoServiceDepth != 0u) &&
|
|
(false == seatDemoCalibrationWorker))
|
|
{
|
|
return;
|
|
}
|
|
seatDemoServiceDepth++;
|
|
OtaCan_Service();
|
|
|
|
if(false != seatDemoStopAllFromIsr)
|
|
{
|
|
uint32_t interruptState = Cy_SysLib_EnterCriticalSection();
|
|
uint16_t sequence = seatDemoStopAllSequenceFromIsr;
|
|
seat_demo_command_t stopCommand = { 0u };
|
|
seatDemoStopAllFromIsr = false;
|
|
Cy_SysLib_ExitCriticalSection(interruptState);
|
|
stopCommand.opcode = SEAT_DEMO_OPCODE_STOP_ALL;
|
|
stopCommand.motor = 0u;
|
|
stopCommand.direction = MOTOR_DIRECTION_STOPPED;
|
|
stopCommand.sequence = sequence;
|
|
/* The emergency mailbox is also the authoritative sequence reset when
|
|
* the normal ring was full. Commit the complete STOP command before
|
|
* accepting any following absolute move. */
|
|
seatDemoLastSequenceValid = true;
|
|
seatDemoLastCommandValid = true;
|
|
seatDemoSequenceReference = sequence;
|
|
seatDemoLastCommand = stopCommand;
|
|
SeatDemoCompat_StopNativeOwnersForAll();
|
|
SeatDemoCompat_StopAllInternal(sequence);
|
|
SeatDemoCompat_SetResponse(
|
|
SEAT_DEMO_ACK_STOP_ALL_ACCEPTED,
|
|
SEAT_DEMO_REJECT_NONE,
|
|
&stopCommand);
|
|
}
|
|
while(false != OtaCan_PopFrame(&frame))
|
|
{
|
|
if(frame.identifier == SEAT_DEMO_CAN_COMMAND_ID)
|
|
{
|
|
/* v40 uses the release/OTA record for software version selection.
|
|
* A legacy App probe is not a motor command. Consume it silently,
|
|
* without changing command sequence/ACK state or queuing 0x306. */
|
|
if(false == SeatDemo_IsLegacyVersionQuery(&frame))
|
|
{
|
|
SeatDemoCompat_HandleCommand(&frame);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
SeatDemoThermal_HandleFrame(&frame);
|
|
}
|
|
OtaCan_Service();
|
|
}
|
|
|
|
SeatDemoDynamic_Service();
|
|
SeatDemoCompat_ServiceTasks();
|
|
if(false == seatDemoCalibrationWorker)
|
|
{
|
|
SeatDemoCompat_ServiceDispatcher();
|
|
}
|
|
SeatDemoThermal_Service();
|
|
SeatDemoCompat_ServiceFeedback();
|
|
seatDemoServiceDepth--;
|
|
}
|
|
|
|
void SeatDemoCompat_ReleaseAxisForNative(uint32_t axis)
|
|
{
|
|
if((axis >= SEAT_DEMO_MOTOR_COUNT) ||
|
|
(false == seatDemoInitialized))
|
|
{
|
|
return;
|
|
}
|
|
if(false != SeatDemoDynamic_IsActive())
|
|
{
|
|
SeatDemoDynamic_Stop(seatDemoSequenceReference);
|
|
}
|
|
if((false != seatDemoTasks[axis].active) ||
|
|
(false != seatDemoTasks[axis].pending))
|
|
{
|
|
SeatDemoCompat_StopAxis(
|
|
axis, seatDemoTasks[axis].command.sequence);
|
|
}
|
|
}
|
|
|
|
void SeatDemoCompat_StopAllFromNative(void)
|
|
{
|
|
uint32_t axis;
|
|
|
|
if(false == seatDemoInitialized)
|
|
{
|
|
return;
|
|
}
|
|
SeatDemoDynamic_Stop(seatDemoSequenceReference);
|
|
for(axis = 0ul; axis < SEAT_DEMO_MOTOR_COUNT; axis++)
|
|
{
|
|
if((false != seatDemoTasks[axis].active) ||
|
|
(false != seatDemoTasks[axis].pending))
|
|
{
|
|
SeatDemoCompat_StopAxis(
|
|
axis, seatDemoTasks[axis].command.sequence);
|
|
}
|
|
}
|
|
}
|
|
|
|
void SeatDemoCompat_PrepareForBootloader(void)
|
|
{
|
|
SeatDemoCompat_StopAllFromNative();
|
|
SeatDemoThermal_ForceSafeOff();
|
|
}
|
|
|
|
bool SeatDemoCompat_IsAxisOwned(uint32_t axis)
|
|
{
|
|
return (axis < SEAT_DEMO_MOTOR_COUNT) &&
|
|
((false != SeatDemoDynamic_IsActive()) ||
|
|
(false != seatDemoTasks[axis].active) ||
|
|
(false != seatDemoTasks[axis].pending));
|
|
}
|
|
|
|
bool SeatDemoCompat_IsAnyMotorOwned(void)
|
|
{
|
|
return (false != SeatDemoDynamic_IsActive()) ||
|
|
(SeatDemoCompat_ActiveMask() != 0u);
|
|
}
|
|
|
|
uint8_t SeatDemoCompat_StartNativeMove(
|
|
uint8_t sequence,
|
|
uint32_t axis,
|
|
uint32_t targetPermille,
|
|
uint8_t dutyPercent)
|
|
{
|
|
seat_demo_command_t command = { 0u };
|
|
|
|
if((axis >= SEAT_DEMO_MOTOR_COUNT) ||
|
|
(targetPermille > 1000ul) ||
|
|
((dutyPercent != 0u) &&
|
|
((dutyPercent < MOTOR_FIXED_DUTY_MIN_PERCENT) ||
|
|
(dutyPercent > MOTOR_FIXED_DUTY_MAX_PERCENT))))
|
|
{
|
|
return (uint8_t)OTA_STATUS_BAD_ARGUMENT;
|
|
}
|
|
if((false == seatDemoInitialized) ||
|
|
(false != driverFaultLatched) ||
|
|
(false == SeatDemoCompat_PositionValid(axis)) ||
|
|
(false != SeatDemoCompat_CalibrationBusy()) ||
|
|
(false != SeatDemoCompat_NativeJobBlocksMotion(axis, false)))
|
|
{
|
|
return (uint8_t)OTA_STATUS_BAD_STATE;
|
|
}
|
|
|
|
SeatDemoCompat_ReleaseAxisForNative(axis);
|
|
command.opcode = SEAT_DEMO_OPCODE_MOVE_ABSOLUTE;
|
|
command.motor = (uint8_t)axis;
|
|
command.direction = MOTOR_DIRECTION_STOPPED;
|
|
command.sequence = (uint16_t)sequence;
|
|
command.paramA = (uint16_t)(targetPermille * 10ul);
|
|
command.paramB = 0u;
|
|
command.dutyPercent = dutyPercent;
|
|
command.explicitDuty = (dutyPercent != 0u);
|
|
SeatDemoCompat_QueueCommand(&command);
|
|
return (uint8_t)OTA_STATUS_OK;
|
|
}
|
|
|
|
#include "seat_demo_thermal.inc"
|