void motor_driver_update_trajectory(motor_driver_t *d, trajectory_chunk_t *traj)
{
chBSemWait(&d->lock);
if (d->control_mode != MOTOR_CONTROL_MODE_TRAJECTORY) {
d->setpt.trajectory = chPoolAlloc(d->traj_buffer_pool);
float *traj_mem = chPoolAlloc(d->traj_buffer_points_pool);
if (d->setpt.trajectory == NULL || traj_mem == NULL) {
chSysHalt("motor driver out of memory (trajectory buffer allocation)");
}
trajectory_init(d->setpt.trajectory, traj_mem, d->traj_buffer_nb_points, 4, traj->sampling_time_us);
d->control_mode = MOTOR_CONTROL_MODE_TRAJECTORY;
}
int ret = trajectory_apply_chunk(d->setpt.trajectory, traj);
switch (ret) {
case TRAJECTORY_ERROR_TIMESTEP_MISMATCH:
chSysHalt("TRAJECTORY_ERROR_TIMESTEP_MISMATCH");
break;
case TRAJECTORY_ERROR_CHUNK_TOO_OLD:
chSysHalt("TRAJECTORY_ERROR_CHUNK_TOO_OLD");
break;
case TRAJECTORY_ERROR_DIMENSION_MISMATCH:
chSysHalt("TRAJECTORY_ERROR_DIMENSION_MISMATCH");
break;
case TRAJECTORY_ERROR_CHUNK_OUT_OF_ORER:
log_message("TRAJECTORY_ERROR_CHUNK_OUT_OF_ORER");
// chSysHalt("TRAJECTORY_ERROR_CHUNK_OUT_OF_ORER");
break;
}
d->update_period = MOTOR_CONTROL_UPDATE_PERIOD_TRAJECTORY;
chBSemSignal(&d->lock);
}
/**
* @brief Guard code for @p chSysUnlockFromIsr().
*
* @notapi
*/
void _dbg_check_unlock_from_isr(void) {
if ((ch.dbg.isr_cnt <= (cnt_t)0) || (ch.dbg.lock_cnt <= (cnt_t)0)) {
chSysHalt("SV#7", __func__);
}
_dbg_leave_lock();
}
/*lint -save -e9075 [8.4] All symbols are invoked from asm context.*/
void _unhandled_exception(void) {
/*lint -restore*/
chSysHalt("unhandled exception");
while (true) {
}
}
/*
* DAC error callback.
*/
static void error_cb1(DACDriver *dacp, dacerror_t err) {
(void)dacp;
(void)err;
chSysHalt("DAC failure");
}
/**
* @brief Guard code for @p chSysUnlock().
*
* @notapi
*/
void _dbg_check_unlock(void) {
if ((ch.dbg.isr_cnt != (cnt_t)0) || (ch.dbg.lock_cnt <= (cnt_t)0)) {
chSysHalt("SV#5");
}
_dbg_leave_lock();
}
/**
* @brief Guard code for @p chSysLock().
*
* @notapi
*/
void _dbg_check_lock(void) {
if ((ch.dbg.isr_cnt != (cnt_t)0) || (ch.dbg.lock_cnt != (cnt_t)0)) {
chSysHalt("SV#4");
}
_dbg_enter_lock();
}
int _kill(int a, int b) {
(void)a;
(void)b;
chSysHalt();
return -1;
}
/* Called upon DAC error */
static void dac_error_callback(DACDriver* dacp, dacerror_t err)
{
(void)dacp;
(void)err;
chSysHalt("Panic: DAC Error.");
while(1);
}
/**
* @brief Guard code for @p chSysUnlockFromIsr().
*
* @notapi
*/
void _dbg_check_unlock_from_isr(void) {
if ((nil.isr_cnt <= (cnt_t)0) || (nil.lock_cnt <= (cnt_t)0)) {
chSysHalt("SV#7");
}
_dbg_leave_lock();
}
float motor_driver_get_position_setpt(motor_driver_t *d)
{
if (d->control_mode != MOTOR_CONTROL_MODE_POSITION) {
chSysHalt("motor driver get position wrong setpt mode");
}
return d->setpt.position;
}
/**
* @brief Guard code for @p chSysLockFromIsr().
*
* @notapi
*/
void _dbg_check_lock_from_isr(void) {
if ((nil.isr_cnt <= (cnt_t)0) || (nil.lock_cnt != (cnt_t)0)) {
chSysHalt("SV#6");
}
_dbg_enter_lock();
}
/**
* @brief Low level HAL driver initialization.
*
* @notapi
*/
void hal_lld_init(void) {
extern void _vectors(void);
uint32_t reg;
/* The system is switched to the RUN0 mode, the default for normal
operations.*/
if (halSPCSetRunMode(SPC5_RUNMODE_RUN0) == CH_FAILED)
chSysHalt();
/* INTC initialization, software vector mode, 4 bytes vectors, starting
at priority 0.*/
INTC.MCR.R = 0;
INTC.CPR.R = 0;
INTC.IACKR.R = (uint32_t)_vectors;
/* PIT channel 0 initialization for Kernel ticks, the PIT is configured
to run in DRUN,RUN0...RUN3 and HALT0 modes, the clock is gated in other
modes.*/
INTC.PSR[59].R = SPC5_PIT0_IRQ_PRIORITY;
halSPCSetPeripheralClockMode(92,
SPC5_ME_PCTL_RUN(2) | SPC5_ME_PCTL_LP(2));
reg = halSPCGetSystemClock() / CH_FREQUENCY - 1;
PIT.PITMCR.R = 1; /* PIT clock enabled, stop while debugging. */
PIT.CH[0].LDVAL.R = reg;
PIT.CH[0].CVAL.R = reg;
PIT.CH[0].TFLG.R = 1; /* Interrupt flag cleared. */
PIT.CH[0].TCTRL.R = 3; /* Timer active, interrupt enabled. */
}
/**
* @brief Guard code for @p chSysUnlockFromIsr().
*
* @notapi
*/
void _dbg_check_unlock_from_isr(void) {
if ((ch.dbg.isr_cnt <= 0) || (ch.dbg.lock_cnt <= 0)) {
chSysHalt("SV#7");
}
_dbg_leave_lock();
}
/**
* @brief Guard code for @p chSysLockFromIsr().
*
* @notapi
*/
void _dbg_check_lock_from_isr(void) {
if ((ch.dbg.isr_cnt <= (cnt_t)0) || (ch.dbg.lock_cnt != (cnt_t)0)) {
chSysHalt("SV#6", __func__);
}
_dbg_enter_lock();
}
/*
* Application entry point.
*/
int main(void) {
msg_t status = RDY_TIMEOUT;
halInit();
chSysInit();
chBSemInit(&alarm_sem, TRUE);
rtcGetTime(&RTCD1, ×pec);
alarmspec.tv_sec = timespec.tv_sec + RTC_ALARMPERIOD;
rtcSetAlarm(&RTCD1, 0, &alarmspec);
rtcSetCallback(&RTCD1, rtc_cb);
while (TRUE) {
/* Wait until alarm callback signaled semaphore.*/
status = chBSemWaitTimeout(&alarm_sem, S2ST(RTC_ALARMPERIOD + 10));
if (status == RDY_TIMEOUT) {
chSysHalt();
}
else {
rtcGetTime(&RTCD1, ×pec);
alarmspec.tv_sec = timespec.tv_sec + RTC_ALARMPERIOD;
rtcSetAlarm(&RTCD1, 0, &alarmspec);
}
}
return 0;
}
void motor_driver_get_trajectory_point(motor_driver_t *d,
int64_t timestamp_us,
float *position,
float *velocity,
float *acceleration,
float *torque)
{
if (d->control_mode != MOTOR_CONTROL_MODE_TRAJECTORY) {
chSysHalt("motor driver get trajectory wrong setpt mode");
}
float *t = trajectory_read(d->setpt.trajectory, timestamp_us);
if (t == NULL) {
// chSysHalt("control error"); // todo
log_message("trajectory read: %d failed", timestamp_get());
*position = 0;
*velocity = 0;
*acceleration = 0;
*torque = 0;
return;
}
*position = t[0];
*velocity = t[1];
*acceleration = t[2];
*torque = t[3];
}
void message_server_thread(void *arg)
{
struct netconn *conn;
struct netbuf *buf;
static uint8_t buffer[4096];
err_t err;
LWIP_UNUSED_ARG(arg);
chRegSetThreadName("rpc_message");
conn = netconn_new(NETCONN_UDP);
if (conn == NULL) {
chSysHalt("Cannot create SimpleRPC message server connection (out of memory).");
}
netconn_bind(conn, NULL, MSG_SERVER_PORT);
while (1) {
err = netconn_recv(conn, &buf);
if (err == ERR_OK) {
netbuf_copy(buf, buffer, buf->p->tot_len);
message_process(buffer, buf->p->tot_len, message_callbacks, message_callbacks_len);
}
netbuf_delete(buf);
}
}
float motor_driver_get_voltage_setpt(motor_driver_t *d)
{
if (d->control_mode != MOTOR_CONTROL_MODE_VOLTAGE) {
chSysHalt("motor driver get voltage wrong setpt mode");
}
return d->setpt.voltage;
}
float motor_driver_get_torque_setpt(motor_driver_t *d)
{
if (d->control_mode != MOTOR_CONTROL_MODE_TORQUE) {
chSysHalt("motor driver get torque wrong setpt mode");
}
return d->setpt.torque;
}
float motor_driver_get_velocity_setpt(motor_driver_t *d)
{
if (d->control_mode != MOTOR_CONTROL_MODE_VELOCITY) {
chSysHalt("motor driver get velocity wrong setpt mode");
}
return d->setpt.velocity;
}
/**
* @brief Guard code for @p chSysLockFromIsr().
*
* @notapi
*/
void _dbg_check_lock_from_isr(void) {
if ((ch.dbg.isr_cnt <= 0) || (ch.dbg.lock_cnt != 0)) {
chSysHalt("SV#6");
}
_dbg_enter_lock();
}
void _exit(int a) {
(void)a;
chSysHalt();
for (;;) {
}
}
/*
* Application entry point.
*/
int main(void) {
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
/*
* Starting the I2C driver 2.
*/
i2cStart(&I2CD2, &i2cconfig);
/*
* Starting the blinker thread.
*/
chThdCreateStatic(blinker_wa, sizeof(blinker_wa),
NORMALPRIO-1, blinker, NULL);
/*
* Normal main() thread activity, in this demo it does nothing.
*/
while (TRUE) {
unsigned i;
msg_t msg;
static const uint8_t cmd[] = {0, 0};
uint8_t data[16];
msg = i2cMasterTransmitTimeout(&I2CD2, 0x52, cmd, sizeof(cmd),
data, sizeof(data), TIME_INFINITE);
msg = msg;
if (msg != RDY_OK)
chSysHalt();
for (i = 0; i < 256; i++) {
msg = i2cMasterReceiveTimeout(&I2CD2, 0x52,
data, sizeof(data), TIME_INFINITE);
if (msg != RDY_OK)
chSysHalt();
}
chThdSleepMilliseconds(500);
}
return 0;
}
/**
* @brief Guard code for @p CH_IRQ_EPILOGUE().
*
* @notapi
*/
void _dbg_check_leave_isr(void) {
port_lock_from_isr();
if ((ch.dbg_isr_cnt <= 0) || (ch.dbg_lock_cnt != 0))
chSysHalt("SV#9");
ch.dbg_isr_cnt--;
port_unlock_from_isr();
}
/**
* @brief Guard code for @p CH_IRQ_PROLOGUE().
*
* @notapi
*/
void _dbg_check_enter_isr(void) {
port_lock_from_isr();
if ((ch.dbg_isr_cnt < 0) || (ch.dbg_lock_cnt != 0))
chSysHalt("SV#8");
ch.dbg_isr_cnt++;
port_unlock_from_isr();
}
/**
* @brief Start a thread by invoking its work function.
* @details If the work function returns @p chThdExit() is automatically
* invoked.
*/
void _port_thread_start(void) {
chSysUnlock();
asm volatile ("movw r24, r4");
asm volatile ("movw r30, r2");
asm volatile ("icall");
chSysHalt(0);
}
/**
* @brief Guard code for @p CH_IRQ_PROLOGUE().
*
* @notapi
*/
void _dbg_check_enter_isr(void) {
port_lock_from_isr();
if ((nil.isr_cnt < (cnt_t)0) || (nil.lock_cnt != (cnt_t)0)) {
chSysHalt("SV#8");
}
nil.isr_cnt++;
port_unlock_from_isr();
}
/**
* @brief Guard code for @p CH_IRQ_EPILOGUE().
*
* @notapi
*/
void _dbg_check_leave_isr(void) {
port_lock_from_isr();
if ((nil.isr_cnt <= (cnt_t)0) || (nil.lock_cnt != (cnt_t)0)) {
chSysHalt("SV#9");
}
nil.isr_cnt--;
port_unlock_from_isr();
}
/**
* @brief Guard code for @p CH_IRQ_EPILOGUE().
*
* @notapi
*/
void _dbg_check_leave_isr(void) {
port_lock_from_isr();
if ((ch.dbg.isr_cnt <= (cnt_t)0) || (ch.dbg.lock_cnt != (cnt_t)0)) {
chSysHalt("SV#9", __func__);
}
ch.dbg.isr_cnt--;
port_unlock_from_isr();
}
/**
* @brief Guard code for @p CH_IRQ_PROLOGUE().
*
* @notapi
*/
void _dbg_check_enter_isr(void) {
port_lock_from_isr();
if ((ch.dbg.isr_cnt < (cnt_t)0) || (ch.dbg.lock_cnt != (cnt_t)0)) {
chSysHalt("SV#8", __func__);
}
ch.dbg.isr_cnt++;
port_unlock_from_isr();
}
