void makeOneBeep(int freqNr, int duration_in_seconds){
long ticks_between_toggle = rt_timer_ns2ticks((1000000000/2)/freq[freqNr]);
RTIME next_toggle_time = rt_timer_read();
RTIME stop_time = rt_timer_ns2ticks(duration_in_seconds*1000000000)+next_toggle_time;
while(next_toggle_time < stop_time){
next_toggle_time+=ticks_between_toggle;
rt_task_sleep_until(next_toggle_time);
toggleSpeaker();
}
}
void testtask(void *cookie){
int count = 0;
int ret;
unsigned long overrun;
ret = rt_task_set_periodic(NULL, TM_NOW, rt_timer_ns2ticks(task_period_ns));
if (ret) {
printf("error while set periodic, code %d\n",ret);
return;
}
while(!end){
ret = rt_task_set_mode(0, T_CONFORMING, NULL);
//ret = rt_task_set_mode( 0,T_RRB,0 );
if (ret) {
printf("error while rt_task_set_mode, code %d\n",ret);
return;
}
ret = rt_task_wait_period(&overrun);
if (ret) {
printf("error while rt_task_wait_period, code %d\n",ret);
return;
}
count++;
printf("message from testtask: count=%d\n", count);
fflush(NULL);
}
}
void event(void *cookie)
{
int err;
err = rt_task_set_periodic(NULL,
TM_NOW,
rt_timer_ns2ticks(sampling_period));
if (err) {
fprintf(stderr,"switch: failed to set periodic, code %d\n", err);
return;
}
for (;;) {
err = rt_task_wait_period(NULL);
if (err) {
if (err != -ETIMEDOUT) {
/* Timer stopped. */
rt_task_delete(NULL);
}
}
switch_count++;
switch_tsc = rt_timer_tsc();
rt_sem_v(&switch_sem);
}
}
static void event(void *cookie)
{
int err;
err = rt_task_set_periodic(NULL,
TM_NOW,
rt_timer_ns2ticks(sampling_period));
if (err) {
warning("failed to enter periodic timing (%s)\n",
symerror(err));
return;
}
for (;;) {
err = rt_task_wait_period(NULL);
if (err) {
if (err != -ETIMEDOUT)
exit(EXIT_FAILURE);
late++;
}
switch_count++;
err = rt_sem_broadcast(&switch_sem);
switch_tsc = rt_timer_tsc();
if (err) {
if (err != -EIDRM && err != -EINVAL)
warning("failed to broadcast semaphore (%s)\n",
symerror(err));
break;
}
}
}
int RT::OS::setPeriod(RT::OS::Task task,long long period)
{
// Retrieve task struct
xenomai_task_t *t = reinterpret_cast<xenomai_task_t *>(task);
// Set wake up limits
if(period/10 > 50000ll)
t->wakeup_t = rt_timer_ns2ticks(50000ll);
else
t->wakeup_t = rt_timer_ns2ticks(period/10);
// Setup timing bounds for oneshot operation
t->period = rt_timer_ns2ticks(period);
t->next_t = rt_timer_read() + t->period;
return 0;
}
INTERNAL_QUAL void rtos_task_make_periodic(RTOS_TASK* mytask, NANO_TIME nanosecs )
{
if (nanosecs == 0) {
rt_task_set_periodic( &(mytask->xenotask), TM_NOW, TM_INFINITE);
}
else {
rt_task_set_periodic( &(mytask->xenotask), TM_NOW, rt_timer_ns2ticks(nanosecs) );
}
}
static void rt_task(void)
{
int i, j, ret;
for (i = 0; i < loops; i++) {
rt_task_sleep(rt_timer_ns2ticks(delay));
if (count)
memcpy(&frame.data[0], &i, sizeof(i));
/* Note: sendto avoids the definiton of a receive filter list */
if (use_send)
ret = send(s, (void *)&frame, sizeof(can_frame_t), 0);
else
ret = sendto(s, (void *)&frame, sizeof(can_frame_t), 0,
(struct sockaddr *)&to_addr, sizeof(to_addr));
if (ret < 0) {
switch (ret) {
case -ETIMEDOUT:
if (verbose)
printf("send(to): timed out");
break;
case -EBADF:
if (verbose)
printf("send(to): aborted because socket was closed");
break;
default:
fprintf(stderr, "send: %s\n", strerror(-ret));
break;
}
i = loops; /* abort */
break;
}
if (verbose && (i % print) == 0) {
if (frame.can_id & CAN_EFF_FLAG)
printf("<0x%08x>", frame.can_id & CAN_EFF_MASK);
else
printf("<0x%03x>", frame.can_id & CAN_SFF_MASK);
printf(" [%d]", frame.can_dlc);
for (j = 0; j < frame.can_dlc; j++) {
printf(" %02x", frame.data[j]);
}
printf("\n");
}
}
}
void rt_task_body(void *cookie)
{
RTIME end;
int err;
rt_task_thread = pthread_self();
rt_printf("syscall\n");
setup_checkdebug(SIGDEBUG_MIGRATE_SYSCALL);
sched_yield();
check_sigdebug_received("SIGDEBUG_MIGRATE_SYSCALL");
rt_printf("signal\n");
setup_checkdebug(SIGDEBUG_MIGRATE_SIGNAL);
err = rt_sem_v(&send_signal);
check_no_error("rt_sem_v", err);
rt_task_sleep(rt_timer_ns2ticks(10000000LL));
check_sigdebug_received("SIGDEBUG_MIGRATE_SIGNAL");
rt_printf("relaxed mutex owner\n");
setup_checkdebug(SIGDEBUG_MIGRATE_PRIOINV);
err = rt_mutex_acquire(&prio_invert, TM_INFINITE);
check("rt_mutex_acquire", err, -EINTR);
check_sigdebug_received("SIGDEBUG_MIGRATE_PRIOINV");
rt_printf("page fault\n");
setup_checkdebug(SIGDEBUG_MIGRATE_FAULT);
rt_task_sleep(0);
*mem ^= 0xFF;
check_sigdebug_received("SIGDEBUG_MIGRATE_FAULT");
if (wd) {
rt_printf("watchdog\n");
rt_print_flush_buffers();
setup_checkdebug(SIGDEBUG_WATCHDOG);
end = rt_timer_tsc() + rt_timer_ns2tsc(2100000000ULL);
rt_task_sleep(0);
while (rt_timer_tsc() < end && !sigdebug_received)
/* busy loop */;
check_sigdebug_received("SIGDEBUG_WATCHDOG");
}
}
void LinearMotorControlTask::run()
{
can_frame startBeckhoffFrame;
startBeckhoffFrame.can_id = 0x0;
startBeckhoffFrame.can_dlc = 2;
startBeckhoffFrame.data[0] = 1;
startBeckhoffFrame.data[1] = 1;
can->sendFrame(startBeckhoffFrame);
can_frame answerFrame;
rt_task_set_periodic(NULL, TM_NOW, rt_timer_ns2ticks(1000000));
while (1)
{
can->sendFrame(pdoOneFrame);
can->sendFrame(pdoTwoFrame);
can->sendFrame(pdoThreeFrame);
if (sendStates)
{
can->sendFrame(stateOneFrame);
can->sendFrame(stateTwoFrame);
can->sendFrame(stateThreeFrame);
sendStates = false;
}
for (int i = 0; i < 3; ++i)
{
can->recvFrame(answerFrame);
answerFrameMap[answerFrame.can_id] = answerFrame;
}
/*setAccelerationOne((uint32_t)((double)getAccelerationSetpointOne()*tanh((double)abs(getPositionSetpointOne()-getPositionOne()))));
setAccelerationTwo((uint32_t)((double)getAccelerationSetpointTwo()*tanh((double)abs(getPositionSetpointTwo()-getPositionTwo()))));
setAccelerationThree((uint32_t)((double)getAccelerationSetpointThree()*tanh((double)abs(getPositionSetpointThree()-getPositionThree()))));*/
/*setAccelerationOne((uint32_t)(accUpperBound*tanh((double)abs(getPositionSetpointOne()-getPositionOne())*0.001)));
setAccelerationTwo((uint32_t)(accUpperBound*tanh((double)abs(getPositionSetpointTwo()-getPositionTwo())*0.001)));
setAccelerationThree((uint32_t)(accUpperBound*tanh((double)abs(getPositionSetpointThree()-getPositionThree())*0.001)));*/
rt_task_wait_period(&overruns);
}
}
void latency (void *cookie)
{
int err, count, nsamples;
RTIME expected, period;
err = rt_timer_start(TM_ONESHOT);
if (err)
{
fprintf(stderr,"latency: cannot start timer, code %d\n",err);
return;
}
nsamples = ONE_BILLION / sampling_period;
period = rt_timer_ns2ticks(sampling_period);
expected = rt_timer_tsc();
err = rt_task_set_periodic(NULL,TM_NOW,sampling_period);
if (err)
{
fprintf(stderr,"latency: failed to set periodic, code %d\n",err);
return;
}
for (;;)
{
long minj = TEN_MILLION, maxj = -TEN_MILLION, dt, sumj;
overrun = 0;
test_loops++;
for (count = sumj = 0; count < nsamples; count++)
{
unsigned long ov;
expected += period;
err = rt_task_wait_period(&ov);
if (err)
{
if (err != -ETIMEDOUT)
rt_task_delete(NULL); /* Timer stopped. */
overrun += ov;
}
dt = (long)(rt_timer_tsc() - expected);
if (dt > maxj) maxj = dt;
if (dt < minj) minj = dt;
sumj += dt;
if (!finished && (do_histogram || do_stats))
add_histogram(histogram_avg, dt);
}
if (!finished && (do_histogram || do_stats))
{
add_histogram(histogram_max, maxj);
add_histogram(histogram_min, minj);
}
minjitter = rt_timer_ticks2ns(minj);
maxjitter = rt_timer_ticks2ns(maxj);
avgjitter = rt_timer_ticks2ns(sumj / nsamples);
rt_sem_v(&display_sem);
}
}
void motor_cmd_routine(void *m_arg)
{
int ret;
RT_TIMER_INFO timer_info;
long long task_period;
unsigned long overruns = 0;
int16_t req_current = 0;
int sync_ref_counter=0;
float cos_el;
float sin_el;
float v_req_az;
float V_REQ_AZ = 0;
float P_term_az, error_az;
float p_az = 20.0;
float i_az = 1.0;
static float az_integral = 0.0;
float I_term_az, INTEGRAL_CUTOFF=0.5;
printf("Starting Motor Commanding task\n");
rt_timer_inquire(&timer_info);
if (timer_info.period == TM_ONESHOT)
{
// When using an aperiodic timer, task period is specified in ns
task_period = rt_timer_ns2ticks(1000000000ll / 100);
}
else
{
// When using a periodic timer, task period is specified in number of timer periods
task_period = (1000000000ll / 100) / timer_info.period;
}
ret = rt_task_set_periodic(NULL, TM_NOW, task_period);
if (ret)
{
printf("error while set periodic, code %d\n", ret);
return;
}
// Make sure we are in primary mode before entering the timer loop
rt_task_set_mode(0, T_PRIMARY, NULL);
while (!stop)
{
unsigned long ov;
// Wait for next time period
ret = rt_task_wait_period(&ov);
if (ret && ret != -ETIMEDOUT)
{
printf("error while rt_task_wait_period, code %d (%s)\n", ret,
strerror(-ret));
break;
}
overruns = overruns + ov;
ecrt_master_receive(master);
ecrt_domain_process(domain);
// write application time to master
ecrt_master_application_time(master, rt_timer_tsc2ns(rt_timer_tsc()));
if (sync_ref_counter) {
sync_ref_counter--;
} else {
sync_ref_counter = 1; // sync every cycle
ecrt_master_sync_reference_clock(master);
}
ecrt_master_sync_slave_clocks(master);
/*******************************************************************\
* Card0: Drive the Azimuth Motor (Reaction Wheel) *
\*******************************************************************/
/* Read sin and cos of the inner frame elevation, calculated by mcp */
// cos_el = 1.0; //( COS_EL*0.000030517578125 ) - 1.0;
// sin_el = 0.0; //( SIN_EL*0.000030517578125 ) - 1.0;
//
// v_req_az = 0.0; //(float)(V_REQ_AZ-32768.0)*0.0016276041666666666666666666666667; // = vreq/614.4
//
// //roll, yaw contributions to az both -'ve (?)
// error_az = (gy_ifroll*sin_el + gy_ifyaw*cos_el) + v_req_az;
//
// P_term_az = p_az*error_az;
//
// if( (p_az == 0.0) || (i_az == 0.0) ) {
// az_integral = 0.0;
// } else {
// az_integral = (1.0 - INTEGRAL_CUTOFF)*az_integral + INTEGRAL_CUTOFF*error_az;
// }
//
// I_term_az = az_integral * p_az * i_az;
// if (I_term_az > 100.0) {
// I_term_az = 100.0;
// az_integral = az_integral *0.9;
// }
// if (I_term_az < -100.0) {
// I_term_az = -100.0;
// az_integral = az_integral * 0.9;
// }
// if (P_term_az > 1.0 || P_term_az < -1.0) printf("error_az: %f\tI: %f\tP: %f\n", error_az, I_term_az, P_term_az);
// req_current = 0.5 *(-(P_term_az + I_term_az) ) ;
req_current = 100;
if (req_current > 200)
printf("Error! Requested current is %d\n", req_current);
else {
EC_WRITE_S16(rx_controller_state.current_val, req_current);
}
ecrt_domain_queue(domain);
ecrt_master_send(master);
}
//switch to secondary mode
ret = rt_task_set_mode(T_PRIMARY, 0, NULL);
if (ret)
{
printf("error while rt_task_set_mode, code %d\n", ret);
return;
}
}
void latency (void *cookie)
{
int err, count, nsamples, warmup = 1;
RTIME expected_tsc, period_tsc, start_ticks;
RT_TIMER_INFO timer_info;
err = rt_timer_start(TM_ONESHOT);
if (err)
{
fprintf(stderr,"latency: cannot start timer, code %d\n",err);
return;
}
err = rt_timer_inquire(&timer_info);
if (err)
{
fprintf(stderr,"latency: rt_timer_inquire, code %d\n",err);
return;
}
nsamples = ONE_BILLION / period_ns;
period_tsc = rt_timer_ns2tsc(period_ns);
/* start time: one millisecond from now. */
start_ticks = timer_info.date + rt_timer_ns2ticks(1000000);
expected_tsc = timer_info.tsc + rt_timer_ns2tsc(1000000);
err = rt_task_set_periodic(NULL,start_ticks,rt_timer_ns2ticks(period_ns));
if (err)
{
fprintf(stderr,"latency: failed to set periodic, code %d\n",err);
return;
}
for (;;)
{
long minj = TEN_MILLION, maxj = -TEN_MILLION, dt, sumj;
long overrun = 0;
test_loops++;
for (count = sumj = 0; count < nsamples; count++)
{
expected_tsc += period_tsc;
err = rt_task_wait_period(NULL);
if (err)
{
if (err != -ETIMEDOUT)
{
fprintf(stderr,"latency: wait period failed, code %d\n",err);
rt_task_delete(NULL); /* Timer stopped. */
}
overrun++;
}
dt = (long)(rt_timer_tsc() - expected_tsc);
if (dt > maxj) maxj = dt;
if (dt < minj) minj = dt;
sumj += dt;
if (!(finished || warmup) && (do_histogram || do_stats))
add_histogram(histogram_avg, dt);
}
if(!warmup)
{
if (!finished && (do_histogram || do_stats))
{
add_histogram(histogram_max, maxj);
add_histogram(histogram_min, minj);
}
minjitter = minj;
if(minj < gminjitter)
gminjitter = minj;
maxjitter = maxj;
if(maxj > gmaxjitter)
gmaxjitter = maxj;
avgjitter = sumj / nsamples;
gavgjitter += avgjitter;
goverrun += overrun;
rt_sem_v(&display_sem);
}
if(warmup && test_loops == WARMUP_TIME)
{
test_loops = 0;
warmup = 0;
}
}
}
/*****************************************************************************
******************************************************************************
Function Name : read_user_sensors
Date : Dec 1997
Remarks:
gets sensor readings from the robot and converts them into standard
units
******************************************************************************
Parameters: (i/o = input/output)
raw (o): the raw sensor readings
misc_raw(o): the raw miscellaneous sensor readings
*****************************************************************************/
int read_user_sensors(SL_Jstate *raw, double *misc_raw) {
int rc;
if( (rc =rt_mutex_acquire(&gdc_mutex, rt_timer_ns2ticks(GDC_RT_MUTEX_TIMEOUT))) )
{
printf("MOTOR SERVO>> Error cannot acquire gdc mutex, error code %d\n", rc);
return FALSE;
}
//we just need to update the file with what we received
gdc_network.checkForReceivedMessages();
#ifdef HAS_LOWER_BODY
if(use_feet)
gdc_network.checkFootSensorReceivedMessages();
#endif
//now we translate things into SL happy data
gdc_sl_interface.translateJointStates(gdc_network.gdc_card_states_, raw);
if(use_feet)
gdc_sl_interface.translateMiscSensors(gdc_network.foot_sensor_states_, misc_raw);
//read imu
if(IMU_MODE_DONT_USE == imu_mode)
{
misc_raw[B_Q0_IMU] = 0.5;
misc_raw[B_Q1_IMU] = 0.5;
misc_raw[B_Q2_IMU] = 0.5;
misc_raw[B_Q3_IMU] = -0.5;
}
else
{
SL_quat orientation;
SL_Cstate position;
double unstab_acc[3];
// unstab_acc[0] = 0.0;
// unstab_acc[1] = 0.0;
// unstab_acc[2] = 0.0;
switch (imu_mode) {
case IMU_MODE_RT_STREAM:
double imu_time;
if (!imu_interface_rt.readDataThreadSafe(unstab_acc,
&orientation.ad[1], imu_time)) {
printf("Could not get data from imu\n");
}
memcpy(&(misc_raw[B_XACC_UNSTAB_IMU]), unstab_acc, 3 * sizeof(double));
memcpy(&(misc_raw[B_AD_A_IMU]), &(orientation.ad[_A_]), 3 * sizeof(double));
misc_raw[B_TSTAMP_IMU] = imu_time;
break;
case IMU_MODE_NONRT:
imu_interface_nonrt.readPose(orientation, position, unstab_acc);
memcpy(&(misc_raw[B_XACC_IMU]), &(position.xdd[_X_]), 3 * sizeof(double));
memcpy(&(misc_raw[B_XACC_UNSTAB_IMU]), unstab_acc, 3 * sizeof(double));
memcpy(&(misc_raw[B_AD_A_IMU]), &(orientation.ad[_A_]), 3 * sizeof(double));
memcpy(&(misc_raw[B_Q0_IMU]), &(orientation.q[_Q0_]), 4 * sizeof(double));
break;
case IMU_MODE_NONRT_STREAM:
imu_interface_nonrt_stream.readData(unstab_acc, &orientation.ad[1],
imu_time);
std::cout << "acc " << unstab_acc[0] << "orient " << orientation.ad[1] << std::endl;
memcpy(&(misc_raw[B_XACC_UNSTAB_IMU]), unstab_acc, 3 * sizeof(double));
memcpy(&(misc_raw[B_AD_A_IMU]), &(orientation.ad[_A_]), 3 * sizeof(double));
misc_raw[B_TSTAMP_IMU] = imu_time;
break;
case IMU_MODE_PF_NONRT_STREAM:
pf_imu_interface_nonrt_stream.getAG(unstab_acc, &orientation.ad[1]);
memcpy(&(misc_raw[B_XACC_IMU]), unstab_acc, 3 * sizeof(double));
memcpy(&(misc_raw[B_AD_A_IMU]), &(orientation.ad[_A_]), 3 * sizeof(double));
break;
default:
rt_printf("bad imu_mode\n");
return false;
}
}
//TODO set a default SL value for inactive joints
if( (rc = rt_mutex_release(&gdc_mutex)) )
{
printf("Error cannot release mutex, error code %d\n", rc);
return FALSE;
}
if(!updateOpenGL(raw, misc_raw))
{
///////////////////////////////////////////////////////////////////////
///////////////To avoid output
////////////////////////////////////////////////////////////////////
//printf("ERROR>>SL_user_sensor_proc: cannot update OpenGL\n");
}
return TRUE;
}
/*****************************************************************************
******************************************************************************
Function Name : send_user_commands
Date : April 1999
Remarks:
translates the commands into raw and sends them to the robot
******************************************************************************
Parameters: (i/o = input/output)
commands (i): the structure containing the commands
*****************************************************************************/
int send_user_commands(SL_Jstate *command) {
int rc;
if( (rc =rt_mutex_acquire(&gdc_mutex, rt_timer_ns2ticks(GDC_RT_MUTEX_TIMEOUT))) )
{
printf("Error cannot acquire gdc mutex, error code %d\n", rc);
return FALSE;
}
//! WARNING commands are joint_state does not contain the desired position
SL_Jstate temp_jstate[N_DOFS+1];
for(int i=1; i<=N_DOFS; ++i)
{
temp_jstate[i].th = joint_des_state[i].th;
temp_jstate[i].thd = joint_des_state[i].thd;
temp_jstate[i].u = command[i].u;
//if(i == 18) printf("%f \n", joint_des_state[i].uff);
}
// translate into gdc values
gdc_sl_interface.translateCommands(temp_jstate, joint_state, gdc_network.gdc_card_states_);
//send a position and torque message (either new gains to ONE card or a position command to ALL)
if(change_position_gains)
{
gdc_network.gdc_card_states_[gdc_card_index].position_P_gain_ = (int16_t)new_pid_gains[0];
gdc_network.gdc_card_states_[gdc_card_index].position_I_gain_ = (int16_t)new_pid_gains[1];
gdc_network.gdc_card_states_[gdc_card_index].position_D_gain_ = (int16_t)new_pid_gains[2];
hermes_communication_tools::GDCMsg gain_msg;
gain_msg.setAllParameters(gdc_network.gdc_card_states_[gdc_card_index]);
gdc_network.sendMessage(gain_msg,gdc_card_index);
change_position_gains = false;
}
else if(change_torque_gains)
{
gdc_network.gdc_card_states_[gdc_card_index].torque_P_gain_ = (int16_t)new_pid_gains[0];
gdc_network.gdc_card_states_[gdc_card_index].torque_I_gain_ = (int16_t)new_pid_gains[1];
gdc_network.gdc_card_states_[gdc_card_index].torque_D_gain_ = (int16_t)new_pid_gains[2];
hermes_communication_tools::GDCMsg gain_msg;
gain_msg.setAllParameters(gdc_network.gdc_card_states_[gdc_card_index]);
gdc_network.sendMessage(gain_msg, gdc_card_index);
change_torque_gains = false;
}
else if(change_valve_dac_bias)
{
gdc_network.gdc_card_states_[gdc_card_index].valve_DAC_bias_ = (int16_t)new_bias[0];
gdc_network.gdc_card_states_[gdc_card_index].load_cell_DAC_bias_ = (int16_t)new_bias[1];
hermes_communication_tools::GDCMsg gain_msg;
gain_msg.setAllParameters(gdc_network.gdc_card_states_[gdc_card_index]);
gdc_network.sendMessage(gain_msg,gdc_card_index);
change_valve_dac_bias = false;
}
else if(change_dither)
{
gdc_network.gdc_card_states_[gdc_card_index].valve_dither_amplitude_ = (int16_t)new_bias[0];
gdc_network.gdc_card_states_[gdc_card_index].valve_dither_frequency_ = (int8_t)new_bias[1];
hermes_communication_tools::GDCMsg gain_msg;
gain_msg.setAllParameters(gdc_network.gdc_card_states_[gdc_card_index]);
gdc_network.sendMessage(gain_msg,gdc_card_index);
change_dither = false;
}
else
{
//send position msg command
hermes_communication_tools::GDCMsg position_msg;
position_msg.globalSetDesPosGetActuals(gdc_network.gdc_card_states_);
gdc_network.sendMultiCastMessage(position_msg);
//torque msg DISABLED
// //hack
/*for(int i=0; i<(int)gdc_network.gdc_card_states_.size(); ++i)
{
if(gdc_network.gdc_card_states_[i].dof_number_ != L_KFE &&
gdc_network.gdc_card_states_[i].dof_number_ != L_HFE)
gdc_network.gdc_card_states_[i].desired_torque_ = 0;
}*/
hermes_communication_tools::GDCMsg torque_msg;
torque_msg.globalSetDesTorqueGetActuals(gdc_network.gdc_card_states_);
gdc_network.sendMultiCastMessage(torque_msg);
//valve command
if(valve_controller.computeValveCommands(gdc_network.gdc_card_states_, gdc_sl_interface, command))
{
hermes_communication_tools::GDCMsg valve_msg;
valve_msg.globalSetDesValveGetActuals(gdc_network.gdc_card_states_);
gdc_network.sendMultiCastMessage(valve_msg);
}
else
{
printf("error cannot compute valve commands!!\n");
return FALSE;
}
}
#ifdef HAS_LOWER_BODY
if(use_feet)
{
//ask for foot sensors
hermes_communication_tools::FootSensorMsg foot_msg;
foot_msg.getActuals();
gdc_network.sendFootSensorMessage(foot_msg, 0);
gdc_network.sendFootSensorMessage(foot_msg, 1);
}
#endif
if( (rc = rt_mutex_release(&gdc_mutex)) )
{
printf("Error cannot release mutex, error code %d\n", rc);
return FALSE;
}
return TRUE;
}
int main(int argc, char **argv)
{
char tempname[] = "/tmp/sigdebug-XXXXXX";
char buf[BUFSIZ], dev[BUFSIZ];
RT_TASK main_task, rt_task;
long int start, trash, end;
unsigned char *mayday, *p;
struct sigaction sa;
int old_wd_value;
char r, w, x, s;
int tmp_fd, d;
FILE *maps;
int err;
rt_print_auto_init(1);
if (argc < 2 || strcmp(argv[1], "--skip-watchdog") != 0) {
wd = fopen("/sys/module/xeno_nucleus/parameters/"
"watchdog_timeout", "w+");
if (!wd) {
fprintf(stderr, "FAILURE: no watchdog available and "
"--skip-watchdog not specified\n");
exit(EXIT_FAILURE);
}
err = fscanf(wd, "%d", &old_wd_value);
check("get watchdog", err, 1);
err = fprintf(wd, "2");
check("set watchdog", err, 1);
fflush(wd);
}
maps = fopen("/proc/self/maps", "r");
if (maps == NULL) {
perror("open /proc/self/maps");
exit(EXIT_FAILURE);
}
while (fgets(buf, sizeof(buf), maps)) {
if (sscanf(buf, "%lx-%lx %c%c%c%c %lx %x:%x %d%s\n",
&start, &end, &r, &w, &x, &s, &trash,
&d, &d, &d, dev) == 11
&& r == 'r' && x == 'x'
&& !strcmp(dev, "/dev/rtheap") && end - start == 4096) {
printf("mayday page starting at 0x%lx [%s]\n"
"mayday code:", start, dev);
mayday = (unsigned char *)start;
for (p = mayday; p < mayday + 32; p++)
printf(" %.2x", *p);
printf("\n");
}
}
fclose(maps);
sigemptyset(&sa.sa_mask);
sa.sa_sigaction = sigdebug_handler;
sa.sa_flags = SA_SIGINFO;
sigaction(SIGDEBUG, &sa, NULL);
sa.sa_sigaction = dummy_handler;
sigaction(SIGUSR1, &sa, NULL);
printf("mlockall\n");
munlockall();
setup_checkdebug(SIGDEBUG_NOMLOCK);
err = rt_task_shadow(&main_task, "main_task", 0, 0);
check("rt_task_shadow", err, -EINTR);
check_sigdebug_received("SIGDEBUG_NOMLOCK");
mlockall(MCL_CURRENT | MCL_FUTURE);
errno = 0;
tmp_fd = mkstemp(tempname);
check_no_error("mkstemp", -errno);
unlink(tempname);
check_no_error("unlink", -errno);
mem = mmap(NULL, 1, PROT_READ | PROT_WRITE, MAP_SHARED, tmp_fd, 0);
check_no_error("mmap", -errno);
err = write(tmp_fd, "X", 1);
check("write", err, 1);
err = rt_task_shadow(&main_task, "main_task", 0, 0);
check_no_error("rt_task_shadow", err);
err = rt_mutex_create(&prio_invert, "prio_invert");
check_no_error("rt_mutex_create", err);
err = rt_mutex_acquire(&prio_invert, TM_INFINITE);
check_no_error("rt_mutex_acquire", err);
err = rt_sem_create(&send_signal, "send_signal", 0, S_PRIO);
check_no_error("rt_sem_create", err);
err = rt_task_spawn(&rt_task, "rt_task", 0, 1, T_WARNSW | T_JOINABLE,
rt_task_body, NULL);
check_no_error("rt_task_spawn", err);
err = rt_sem_p(&send_signal, TM_INFINITE);
check_no_error("rt_sem_signal", err);
pthread_kill(rt_task_thread, SIGUSR1);
rt_task_sleep(rt_timer_ns2ticks(20000000LL));
err = rt_mutex_release(&prio_invert);
check_no_error("rt_mutex_release", err);
err = rt_task_join(&rt_task);
check_no_error("rt_task_join", err);
err = rt_mutex_delete(&prio_invert);
check_no_error("rt_mutex_delete", err);
err = rt_sem_delete(&send_signal);
check_no_error("rt_sem_delete", err);
if (wd) {
fprintf(wd, "%d", old_wd_value);
fclose(wd);
}
fprintf(stderr, "Test OK\n");
return 0;
}
void latency(void *cookie)
{
int err, count, nsamples, warmup = 1;
RTIME expected_tsc, period_tsc, start_ticks, fault_threshold;
RT_TIMER_INFO timer_info;
unsigned old_relaxed = 0;
err = rt_timer_inquire(&timer_info);
if (err) {
fprintf(stderr, "latency: rt_timer_inquire, code %d\n", err);
return;
}
fault_threshold = rt_timer_ns2tsc(CONFIG_XENO_DEFAULT_PERIOD);
nsamples = ONE_BILLION / period_ns / 1000;
period_tsc = rt_timer_ns2tsc(period_ns);
/* start time: one millisecond from now. */
start_ticks = timer_info.date + rt_timer_ns2ticks(1000000);
expected_tsc = timer_info.tsc + rt_timer_ns2tsc(1000000);
err =
rt_task_set_periodic(NULL, start_ticks,
rt_timer_ns2ticks(period_ns));
if (err) {
fprintf(stderr, "latency: failed to set periodic, code %d\n",
err);
return;
}
for (;;) {
long minj = TEN_MILLION, maxj = -TEN_MILLION, dt;
long overrun = 0;
long long sumj;
test_loops++;
for (count = sumj = 0; count < nsamples; count++) {
unsigned new_relaxed;
unsigned long ov;
expected_tsc += period_tsc;
err = rt_task_wait_period(&ov);
dt = (long)(rt_timer_tsc() - expected_tsc);
new_relaxed = sampling_relaxed;
if (dt > maxj) {
if (new_relaxed != old_relaxed
&& dt > fault_threshold)
max_relaxed +=
new_relaxed - old_relaxed;
maxj = dt;
}
old_relaxed = new_relaxed;
if (dt < minj)
minj = dt;
sumj += dt;
if (err) {
if (err != -ETIMEDOUT) {
fprintf(stderr,
"latency: wait period failed, code %d\n",
err);
exit(EXIT_FAILURE); /* Timer stopped. */
}
overrun += ov;
expected_tsc += period_tsc * ov;
}
if (freeze_max && (dt > gmaxjitter)
&& !(finished || warmup)) {
xntrace_user_freeze(rt_timer_tsc2ns(dt), 0);
gmaxjitter = dt;
}
if (!(finished || warmup) && need_histo())
add_histogram(histogram_avg, dt);
}
if (!warmup) {
if (!finished && need_histo()) {
add_histogram(histogram_max, maxj);
add_histogram(histogram_min, minj);
}
minjitter = minj;
if (minj < gminjitter)
gminjitter = minj;
maxjitter = maxj;
if (maxj > gmaxjitter)
gmaxjitter = maxj;
avgjitter = sumj / nsamples;
gavgjitter += avgjitter;
goverrun += overrun;
rt_sem_v(&display_sem);
}
if (warmup && test_loops == WARMUP_TIME) {
test_loops = 0;
warmup = 0;
}
}
}
/**
*
* @param oncillaPtr
*/
void executor(void *oncillaPtr) {
int ret;
unsigned long overrun;
OncillaCmd msgBuf;
OncillaRobot *oncilla = static_cast<OncillaRobot*> (oncillaPtr);
sbcp::ScheduledWorkflow & w = oncilla->bus->Scheduled();
for (int i = 0; i < 4; i++) {
w.AppendScheduledDevice(std::tr1::static_pointer_cast<sbcp::Device, sbcp::amarsi::MotorDriver>(oncilla->legs[i]));
oncilla->legs[i]->Motor1().GoalPosition().Set(oncilla->zeroPosRaw[2 * i]);
oncilla->legs[i]->Motor2().GoalPosition().Set(oncilla->zeroPosRaw[2 * i + 1]);
oncilla->servos[i]->SetCommand(oncilla->zeroPosRaw[i + 8]);
}
ret = rt_task_set_periodic(NULL, TM_NOW, rt_timer_ns2ticks(oncilla->executor_task_period_ns));
if (ret) {
std::runtime_error ex("Xenomai task set periodic failed. Error");
throw ex;
}
ret = rt_task_set_mode(0, T_PRIMARY, NULL);
if (ret) {
std::runtime_error ex("Xenomai task set mode failed. Error");
throw ex;
}
RTIME start_time = rt_timer_read(), current_time;
while (true) {
rt_mutex_acquire(&(oncilla->end_executor), TM_INFINITE);
//rt_mutex_release(&(oncilla->end_executor));
ret = rt_task_wait_period(&overrun);
if (ret) {
std::runtime_error ex("Xenomai task wait failed. Error");
//throw ex;
}
ret = rt_queue_read(&(oncilla->posQueue), &msgBuf, sizeof (msgBuf), TM_NONBLOCK);
if (ret >= 0) {
if (msgBuf.id == OncillaCmd::SET_POS) {
oncilla->legs[0]->Motor1().GoalPosition().Set((int16_t) (msgBuf.args.doubleArgs[0] / (2.0 * M_PI) * 4096.0 + oncilla->maxMotorPos[0] / 2.0));
oncilla->legs[0]->Motor2().GoalPosition().Set((int16_t) ((1.0 - msgBuf.args.doubleArgs[1]) * oncilla->maxMotorPos[1]));
oncilla->legs[1]->Motor1().GoalPosition().Set((int16_t) (-msgBuf.args.doubleArgs[2] / (2.0 * M_PI) * 4096.0 + oncilla->maxMotorPos[2] / 2.0));
oncilla->legs[1]->Motor2().GoalPosition().Set((int16_t) (msgBuf.args.doubleArgs[3] * oncilla->maxMotorPos[3]));
oncilla->legs[2]->Motor1().GoalPosition().Set((int16_t) (msgBuf.args.doubleArgs[4] / (2.0 * M_PI) * 4096.0 + oncilla->maxMotorPos[4] / 2.0));
oncilla->legs[2]->Motor2().GoalPosition().Set((int16_t) ((1.0 - msgBuf.args.doubleArgs[5]) * oncilla->maxMotorPos[5]));
oncilla->legs[3]->Motor1().GoalPosition().Set((int16_t) (-msgBuf.args.doubleArgs[6] / (2.0 * M_PI) * 4096.0 + oncilla->maxMotorPos[6] / 2.0));
oncilla->legs[3]->Motor2().GoalPosition().Set((int16_t) (msgBuf.args.doubleArgs[7] * oncilla->maxMotorPos[7]));
for (int i = 0; i < 4; i++)
oncilla->servos[i]->SetCommand(msgBuf.args.doubleArgs[i + 8]);
rt_queue_free(&(oncilla->posQueue), &msgBuf);
} else if (msgBuf.id == OncillaCmd::RESET_TIMER) {
if (oncilla->RT) {
rt_mutex_acquire(&(oncilla->posArrMutexRT), TM_INFINITE);
oncilla->trajectQueue.clear();
rt_mutex_release(&(oncilla->posArrMutexRT));
} else {
pthread_mutex_lock(&(oncilla->posArrMutex));
oncilla->trajectQueue.clear();
pthread_mutex_unlock(&(oncilla->posArrMutex));
rt_task_set_mode(0, T_PRIMARY, NULL);
}
start_time = rt_timer_read();
}
} else if (ret == -EINVAL || ret == -EIDRM) {
break;
} else if (ret != -EWOULDBLOCK) {
std::runtime_error ex("Cannot read command from message queue. Error");
throw ex;
}
try {
w.StartTransfers();
w.WaitForTransfersCompletion();
} catch (sbcp::MultipleTransferError & e) {
std::runtime_error ex("SBCP communication failed. Error");
throw ex;
}
if (oncilla->RT) {
rt_mutex_acquire(&(oncilla->posArrMutexRT), TM_INFINITE);
} else {
// If causing timeout for real-time task try using pthread_mutex_trylock() instead
pthread_mutex_lock(&(oncilla->posArrMutex));
}
for (int i = 0; i < 4; i++) {
oncilla->currentPosRaw[2 * i] = oncilla->legs[i]->Motor1().PresentPosition().Get();
oncilla->currentPosRaw[2 * i + 1] = oncilla->legs[i]->Motor2().PresentPosition().Get();
}
oncilla->currentPos[0] = (double) (oncilla->currentPosRaw[0] - oncilla->maxMotorPos[0] / 2.0) / 4096.0 * (2.0 * M_PI);
oncilla->currentPos[1] = (1.0 - (double) oncilla->currentPosRaw[1] / (double) oncilla->maxMotorPos[1]);
oncilla->currentPos[2] = (double) -(oncilla->currentPosRaw[2] - oncilla->maxMotorPos[2] / 2.0) / 4096.0 * (2.0 * M_PI);
oncilla->currentPos[3] = ((double) oncilla->currentPosRaw[3] / (double) oncilla->maxMotorPos[3]);
oncilla->currentPos[4] = (double) (oncilla->currentPosRaw[4] - oncilla->maxMotorPos[4] / 2.0) / 4096.0 * (2.0 * M_PI);
oncilla->currentPos[5] = (1.0 - (double) oncilla->currentPosRaw[5] / (double) oncilla->maxMotorPos[5]);
oncilla->currentPos[6] = (double) -(oncilla->currentPosRaw[6] - oncilla->maxMotorPos[6] / 2.0) / 4096.0 * (2.0 * M_PI);
oncilla->currentPos[7] = ((double) oncilla->currentPosRaw[7] / (double) oncilla->maxMotorPos[7]);
for (int i = 0; i < 4; i++)
oncilla->currentPos[i + 8] = oncilla->servos[i]->Command();
current_time = rt_timer_read();
OncillaRobot::trajectPoint newTrajectPoint;
newTrajectPoint.coords[0] = (double) (current_time - start_time) / 1000000;
memcpy(newTrajectPoint.coords + 1, oncilla->currentPos, 12 * sizeof (double));
if (oncilla->trajectQueue.size() >= oncilla->queueSize)
oncilla->trajectQueue.pop_front();
oncilla->trajectQueue.push_back(newTrajectPoint);
if (oncilla->RT) {
rt_mutex_release(&(oncilla->posArrMutexRT));
} else {
pthread_mutex_unlock(&(oncilla->posArrMutex));
rt_task_set_mode(0, T_PRIMARY, NULL);
}
rt_mutex_release(&(oncilla->end_executor));
//rt_mutex_acquire(&(oncilla->end_executor), TM_INFINITE);
}
/*rt_mutex_release(&(oncilla->end_executor));
pthread_mutex_lock(&(oncilla->end_mutex));
pthread_cond_signal(&(oncilla->cond));
pthread_mutex_unlock(&(oncilla->end_mutex));
*/
}
void latency (void *cookie)
{
int err, count, nsamples, warmup = 1;
RTIME expected_tsc, period_tsc, start_ticks;
RT_TIMER_INFO timer_info;
RT_QUEUE q;
rt_queue_create(&q, "queue", 0, 100, 0);
if (!(hard_timer_running = rt_is_hard_timer_running())) {
err = rt_timer_start(TM_ONESHOT);
if (err)
{
fprintf(stderr,"latency: cannot start timer, code %d\n",err);
return;
}
}
err = rt_timer_inquire(&timer_info);
if (err)
{
fprintf(stderr,"latency: rt_timer_inquire, code %d\n",err);
return;
}
nsamples = ONE_BILLION / period_ns / 1;
period_tsc = rt_timer_ns2tsc(period_ns);
/* start time: one millisecond from now. */
start_ticks = timer_info.date + rt_timer_ns2ticks(1000000);
expected_tsc = timer_info.tsc + rt_timer_ns2tsc(1000000);
err = rt_task_set_periodic(NULL,start_ticks,period_ns);
if (err)
{
fprintf(stderr,"latency: failed to set periodic, code %d\n",err);
return;
}
for (;;)
{
long minj = TEN_MILLION, maxj = -TEN_MILLION, dt, sumj;
long overrun = 0;
test_loops++;
for (count = sumj = 0; count < nsamples; count++)
{
expected_tsc += period_tsc;
err = rt_task_wait_period(NULL);
if (err)
{
if (err != -ETIMEDOUT) {
rt_queue_delete(&q);
rt_task_delete(NULL); /* Timer stopped. */
}
overrun++;
}
dt = (long)(rt_timer_tsc() - expected_tsc);
if (dt > maxj) maxj = dt;
if (dt < minj) minj = dt;
sumj += dt;
if (!(finished || warmup) && (do_histogram || do_stats))
add_histogram(histogram_avg, dt);
}
if(!warmup)
{
if (!finished && (do_histogram || do_stats))
{
add_histogram(histogram_max, maxj);
add_histogram(histogram_min, minj);
}
minjitter = minj;
if(minj < gminjitter)
gminjitter = minj;
maxjitter = maxj;
if(maxj > gmaxjitter)
gmaxjitter = maxj;
avgjitter = sumj / nsamples;
gavgjitter += avgjitter;
goverrun += overrun;
rt_sem_v(&display_sem);
struct smpl_t { long minjitter, avgjitter, maxjitter, overrun; } *smpl;
smpl = rt_queue_alloc(&q, sizeof(struct smpl_t));
#if 1
smpl->minjitter = rt_timer_tsc2ns(minj);
smpl->maxjitter = rt_timer_tsc2ns(maxj);
smpl->avgjitter = rt_timer_tsc2ns(sumj / nsamples);
smpl->overrun = goverrun;
rt_queue_send(&q, smpl, sizeof(struct smpl_t), TM_NONBLOCK);
#endif
}
if(warmup && test_loops == WARMUP_TIME)
{
test_loops = 0;
warmup = 0;
}
}
}
void PLC_SetTimer(unsigned long long next, unsigned long long period)
{
RTIME current_time = rt_timer_read();
rt_task_set_periodic(&PLC_task, current_time + next, rt_timer_ns2ticks(period));
}
