/** * drm_atomic_helper_commit - commit validated state object * @dev: DRM device * @state: the driver state object * @async: asynchronous commit * * This function commits a with drm_atomic_helper_check() pre-validated state * object. This can still fail when e.g. the framebuffer reservation fails. For * now this doesn't implement asynchronous commits. * * RETURNS * Zero for success or -errno. */ int msm_atomic_commit(struct drm_device *dev, struct drm_atomic_state *state, bool async) { int nplanes = dev->mode_config.num_total_plane; int ncrtcs = dev->mode_config.num_crtc; struct msm_commit *c; int i, ret; ret = drm_atomic_helper_prepare_planes(dev, state); if (ret) return ret; c = new_commit(state); if (!c) return -ENOMEM; /* * Figure out what crtcs we have: */ for (i = 0; i < ncrtcs; i++) { struct drm_crtc *crtc = state->crtcs[i]; if (!crtc) continue; c->crtc_mask |= (1 << drm_crtc_index(crtc)); } /* * Figure out what fence to wait for: */ for (i = 0; i < nplanes; i++) { struct drm_plane *plane = state->planes[i]; struct drm_plane_state *new_state = state->plane_states[i]; if (!plane) continue; if ((plane->state->fb != new_state->fb) && new_state->fb) add_fb(c, new_state->fb); } /* * Wait for pending updates on any of the same crtc's and then * mark our set of crtc's as busy: */ ret = start_atomic(dev->dev_private, c->crtc_mask); if (ret) return ret; /* * This is the point of no return - everything below never fails except * when the hw goes bonghits. Which means we can commit the new state on * the software side now. */ drm_atomic_helper_swap_state(dev, state); /* * Everything below can be run asynchronously without the need to grab * any modeset locks at all under one conditions: It must be guaranteed * that the asynchronous work has either been cancelled (if the driver * supports it, which at least requires that the framebuffers get * cleaned up with drm_atomic_helper_cleanup_planes()) or completed * before the new state gets committed on the software side with * drm_atomic_helper_swap_state(). * * This scheme allows new atomic state updates to be prepared and * checked in parallel to the asynchronous completion of the previous * update. Which is important since compositors need to figure out the * composition of the next frame right after having submitted the * current layout. */ if (async) { msm_queue_fence_cb(dev, &c->fence_cb, c->fence); return 0; } ret = msm_wait_fence_interruptable(dev, c->fence, NULL); if (ret) { WARN_ON(ret); // TODO unswap state back? or?? kfree(c); return ret; } complete_commit(c); return 0; }
/******************************************************************************* ******************************************************************************* \note run_traj_task \date Jun. 1999 \remarks run the task from the task servo: REAL TIME requirements! ******************************************************************************* Function Parameters: [in]=input,[out]=output none ******************************************************************************/ static int run_traj_task(void) { int j, i; double time_of_traj_index; double time_of_next_traj_index; static int flag_reset = FALSE; static double dist = 1e-1; static int num_it = 1; static int firsttime = TRUE; static double tauReturn = 1.5; const int returnLen = (int)(tauReturn * task_servo_rate); static int return_iter = 1; static int return_total_len = 1; static SL_Jstate j0[N_DOFS+1]; static SL_Jstate j1[N_DOFS+1]; static Matrix trajReturn; static double time1 = 0; static double time2 = 0; // checking for limits if (!check_joint_limits(joint_state, 0.01)) { printf("Joint limits are exceeding soft limits! Freezing...\n"); freeze(); } if (!check_des_joint_limits(joint_des_state, 0.01)) { printf("Joint des limits are exceeding soft limits! Freezing...\n"); freeze(); } // calculate the racket orientation from endeffector calc_racket(&racket_state, &racket_orient, cart_state[RIGHT_HAND], cart_orient[RIGHT_HAND]); if(collision_detection(racket_state)){ printf("Collision detected! Freezing...\n"); freeze(); } if (firsttime) { firsttime = FALSE; traj_index = 1; //print_vec("q0:",q0); //turn_off_integrator(); // keep integrators off throughout the trajectory bzero((void *)j0, sizeof(SL_Jstate)* (N_DOFS+1)); for (i = 1; i <= N_DOFS; i++) { j0[i].th = q0[i]; j0[i].thd = 0.0; j0[i].thdd = 0.0; j1[i].th = traj_pos[traj_len][i]; j1[i].thd = traj_vel[traj_len][i]; j1[i].thdd = traj_acc[traj_len][i]; } trajReturn = my_matrix(1,returnLen,1,3*N_DOFS); //entirePoly5th(trajReturn, j1, j0, tauReturn); save_act_traj(traj_index); fprint_joint_act_traj(traj_index); fprint_joint_des_traj(traj_index); fprint_cart_des_traj(traj_index); fprint_cart_act_traj(traj_index); } time_of_traj_index = ((double)(traj_index-1))/SAMP_FREQ; time_of_next_traj_index = ((double)(traj_index))/SAMP_FREQ; /* the statement below takes care of numerical rounding problems */ if (task_time >= time_of_next_traj_index-0.00001 && !flag_reset) { // here we can move on to the next point on the trajectory //printf("Traj index: %d \n", traj_index); traj_index = traj_index + 1; if (traj_index > traj_len) { //traj_index = 1; // revert back to usual PD control to bring it back to starting position toggle_fb(); printf("Number of iter: %d\n", num_it); flag_reset = TRUE; rms_traj_error(traj_pos_act,traj_vel_act,traj_pos,traj_vel); entirePoly5th(trajReturn, joint_state, j0, tauReturn); //entirePoly5th(trajReturn, joint_state, joint_default_state, tauReturn); //return_iter = 1; //task_time = 1./(double)task_servo_rate; //break; } else { // saving the actual joint positions // this will be used by ILC save_act_traj(traj_index); fprint_joint_act_traj(traj_index); fprint_joint_des_traj(traj_index); fprint_cart_des_traj(traj_index); fprint_cart_act_traj(traj_index); } time_of_traj_index = ((double)(traj_index-1))/SAMP_FREQ; time_of_next_traj_index = ((double)(traj_index))/SAMP_FREQ; } // make sure vel and acc are zero if (flag_reset) { //printf("Distance to starting pos: %f\n", euclidian_dist(joint_state,traj_pos[1])); //printf("Resetting...\n"); if (euclidian_dist(joint_state,q0) > dist || return_total_len < 2000) { for (i = 1; i <= N_DOFS; ++i) { joint_des_state[i].th = trajReturn[return_iter][3*i-2]; //q0[i]; joint_des_state[i].thd = trajReturn[return_iter][3*i-1]; joint_des_state[i].thdd = trajReturn[return_iter][3*i]; } if (return_iter < returnLen) { return_iter++; return_total_len++; } else { return_total_len++; if (return_total_len % 500 == 0) { printf("euclidian dist: %f\n", euclidian_dist(joint_state,q0)); } /* if (return_total_len == 1000) { printf("Initialization problem with PD. Turning on PID (if enabled with ck)...\n"); turn_on_integrator(); }*/ } SL_InvDyn(NULL,joint_des_state,endeff,&base_state,&base_orient); if (friction_comp) { addFrictionModel(joint_des_state); } /*if (!check_range(joint_des_state)) { printf("q0 is out of range! Freezing...\n"); freeze(); }*/ // add feedback from task servo //add_fb(traj_index); fprint_joint_act_traj(0); fprint_cart_act_traj(0); } else { if (repeat_flag) { return_iter = 1; return_total_len = 1; traj_index = 1; task_time = 0.0; //1./(double)task_servo_rate; flag_reset = FALSE; //go back to start save_act_traj(traj_index); fprint_joint_act_traj(traj_index); fprint_joint_des_traj(traj_index); fprint_cart_des_traj(traj_index); fprint_cart_act_traj(traj_index); toggle_fb(); // revert to lqr // get feedforward part to be changed if (num_it > 1) {// this is due to extreme good initialization firsttime //turn_on_integrator(); time1 = get_time(); ilc_main(); //update_basic(); time2 = get_time(); printf("ILC takes %.3f ms...\n",(time2-time1)/1000); //turn_off_integrator(); //turn integrators off again } num_it = num_it + 1; } else { printf("Switching to NO TASK!\n"); setTaskByName(NO_TASK); return TRUE; } } } else { // feedforward along the trajectory for (i = 1; i <= N_DOFS; i++) { joint_des_state[i].th = traj_pos[traj_index][i]; joint_des_state[i].thd = traj_vel[traj_index][i]; joint_des_state[i].thdd = traj_acc[traj_index][i]; joint_des_state[i].uff = traj_uff[traj_index][i]; } //SL_InvDyn(joint_state,joint_des_state,endeff,&base_state,&base_orient); task_time += 1./(double)task_servo_rate; if (!check_range(joint_des_state)) { printf("ILC exceeded torque limits. Freezing...\n"); freeze(); } // add feedback from task servo add_fb(traj_index); } return TRUE; }