/**
* 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;
}
