static i32 timer_start(struct sw_timer *swt, struct u64_time *time_u64,
u32 options)
{
if(false == is_alloc_only(swt))
return -1; /* Not only allocated but started as well */
if((-1 == setup_timer_interval(swt, time_u64)) ||
(-1 == setup_timer_exec(swt, options)))
return -1;
set_periodic(swt, options & OPT_TIMER_PERIODIC ? true : false);
return 0;
}
static i32 timer_stop(struct sw_timer *swt)
{
struct hwt_info *hwt = swt->hwt_obj;
if(0 != remove_elem(swt, &hwt->used_list)) {
return -1;
}
set_scheduled(swt, false);
set_periodic(swt, false);
if(NULL != hwt->used_list)
sched_timer_if_new(hwt->used_list);
else
hwt_stop(hwt); /* No pending request, stop HW */
return 0;
}
void EinspurDynamikRealtime::run()
{
motPlat->start();
set_periodic(period);
motPlat->getSendMutex().acquire(period);
motPlat->switchToMode<ValidateMotionPlatform::controlDisabled>();
motPlat->getSendMutex().release();
rt_task_wait_period(&overruns);
//SteeringWheel
set_periodic(TM_INFINITE);
steerWheel->init();
std::deque<int32_t> steerSpeedDeque(50, 0);
std::deque<int32_t>::iterator steerSpeedDequeIt;
double lowPassSpeed = 0.0;
int32_t driftPosition = 0;
set_periodic(period);
while (runTask)
{
brakePedal = std::max(0.0, std::min(1.0, ValidateMotionPlatform::instance()->getBrakePedalPosition() * 5.0));
if (!pause)
{
double h = (double)(period * (overruns + 1)) * 1e-9;
integrate(h, 0, 0, 0);
}
/*longAngleDeque.pop_front();
longAngleDeque.push_back(-yOne.zeta);
double longAngle=0;
for(angleDequeIt=longAngleDeque.begin(); angleDequeIt!=longAngleDeque.end(); ++angleDequeIt) {
longAngle += (*angleDequeIt);
}
longAngle = longAngle/(double)longAngleDeque.size();
latAngleDeque.pop_front();
latAngleDeque.push_back(-yOne.epsilon);
double latAngle=0;
for(angleDequeIt=latAngleDeque.begin(); angleDequeIt!=latAngleDeque.end(); ++angleDequeIt) {
latAngle += (*angleDequeIt);
}
latAngle = latAngle/(double)latAngleDeque.size();*/
if (movingToGround)
{
if (motPlat->isGrounded())
{
movingToGround = false;
motPlat->getSendMutex().acquire(period);
motPlat->switchToMode<ValidateMotionPlatform::controlDisabled>();
motPlat->getSendMutex().release();
}
}
else if (returningToAction)
{
if (motPlat->isMiddleLifted())
{
returningToAction = false;
pause = false;
motPlat->getSendMutex().acquire(period);
//motPlat->switchToMode<ValidateMotionPlatform::controlPositioning>();
motPlat->switchToMode<ValidateMotionPlatform::controlInterpolatedPositioning>();
for (unsigned int motIt = 0; motIt < motPlat->numLinMots; ++motIt)
{
motPlat->setVelocitySetpoint(motIt, ValidateMotionPlatform::velMax);
motPlat->setAccelerationSetpoint(motIt, ValidateMotionPlatform::accMax);
}
motPlat->getSendMutex().release();
}
}
else if (!pause)
{
double longAngle = -yOne.zeta;
double latAngle = -yOne.epsilon;
motPlat->getSendMutex().acquire(period);
motPlat->setLongitudinalAngleSetpoint(longAngle);
motPlat->setLateralAngleSetpoint(latAngle);
motPlat->getSendMutex().release();
}
//SteeringWheel
steerCon->sendSync();
steerCon->recvPDO(1);
//steerPosition = steerWheel->getPosition();
//steerSpeed = steerWheel->getSpeed();
steerWheel->getPositionSpeed(steerPosition, steerSpeed);
*(((uint8_t *)&steerSpeed) + 3) = (*(((uint8_t *)&steerSpeed) + 2) & 0x80) ? 0xff : 0x0;
steerSpeedDeque.pop_front();
steerSpeedDeque.push_back(steerSpeed);
lowPassSpeed = 0;
for (steerSpeedDequeIt = steerSpeedDeque.begin(); steerSpeedDequeIt != steerSpeedDeque.end(); ++steerSpeedDequeIt)
{
lowPassSpeed += (*steerSpeedDequeIt);
}
lowPassSpeed = (double)lowPassSpeed / (double)steerSpeedDeque.size();
double drillElasticity = tanh(fabs(yOne.v));
int32_t current = (int32_t)(-steerWheel->getSpringConstant() * drillElasticity * (double)steerPosition - steerWheel->getDampingConstant() * lowPassSpeed); //Spring-Damping-Model
current += (int32_t)(((rand() / ((double)RAND_MAX)) - 0.5) * steerWheel->getRumbleAmplitude()); //Rumbling
double drillRigidness = 1.0 - drillElasticity;
if ((driftPosition - steerPosition) > 100000 * drillRigidness)
driftPosition = steerPosition + (int32_t)(100000 * drillRigidness);
else if ((driftPosition - steerPosition) < -100000 * drillRigidness)
driftPosition = steerPosition - (int32_t)(100000 * drillRigidness);
current += (int32_t)((double)(driftPosition - steerPosition) * steerWheel->getDrillConstant());
//std::cerr << "drift steerPosition - steerPosition: " << (int32_t)((double)(driftPosition-steerPosition)*0.005) << ", drill current: " << (int32_t)((double)(driftPosition-steerPosition)*Kdrill) << ", Kdrill: " << Kdrill << std::endl;
steerWheel->setCurrent(current);
steerCon->sendPDO();
/*if((count % 1000)==0) {
std::cerr << "Count: " << count << ", overruns: " << overruns << std::endl;
}
++count;*/
if (overruns > 0)
{
std::cerr << "EinspurDynamikRealtime::run(): overruns: " << overruns << std::endl;
}
rt_task_wait_period(&overruns);
}
/*steerCon->sendSync();
rt_task_wait_period(&overruns);
rt_task_wait_period(&overruns);
steerCon->sendSync();*/
motPlat->getSendMutex().acquire(period);
motPlat->switchToMode<ValidateMotionPlatform::controlToGround>();
motPlat->getSendMutex().release();
while (!motPlat->isGrounded())
{
rt_task_wait_period(&overruns);
}
motPlat->getSendMutex().acquire(period);
motPlat->switchToMode<ValidateMotionPlatform::controlDisabled>();
motPlat->getSendMutex().release();
set_periodic(TM_INFINITE);
//SteerWheel
//rt_task_sleep(10000000);
steerWheel->shutdown();
taskFinished = true;
}
