/***************************************************************************** ****************************************************************************** Function Name : run_tt_task Date : Dec 2013 Remarks: run the task from the task servo: REAL TIME requirements! ****************************************************************************** Paramters: (i/o = input/output) none *****************************************************************************/ static int run_mpc_task(void) { int i,j; static int firsttime = TRUE; // time for moving from desired hitting state to desired resting state static double returnToRestTime = 1.0; // time to wait for simulation static double timeMaxWait = 6.0; // the time it takes to reach from initial state (start) to hitting point // = predicted time it takes the ball from current (est.) position to the hitting point static double hitTime; // solely for simulation static double timeSimInit; // reset kalman filter with this variable static int initKF; // we have this new variable in optim task static double landTime; static Vector ballLand; // desired arm configuration, can be used for hitting state static SL_DJstate target_joint_state[N_DOFS+1]; /* thread optimization variables */ static nlopt_thread_data *optim_data; if (firsttime) { firsttime = FALSE; ballLand = my_vector(1,N_CART); set_des_land_param(ballLand, &landTime); timeSimInit = get_time(); initKF = TRUE; resetSim = FALSE; bzero((char *)&(target_joint_state[1]), N_DOFS * sizeof(target_joint_state[1])); optim_data = (nlopt_thread_data *)malloc(sizeof(nlopt_thread_data)); } // estimate ball state with a filter filter_ball_state(&initKF,&resetSim); // lookup + initialize optimization and get the hitting parameters calc_optim_param(ballLand, landTime, target_joint_state, optim_data, &hitTime); // generate movement or calculate next desired step calc_next_state(*optim_data, returnToRestTime, target_joint_state, &hitTime); // check safety and calculate inv dyn control_arm(); // resetting ball in simulation mode if(simulation) { if ((((get_time() - timeSimInit) > timeMaxWait * 1e6) || resetSim) && !moving) { reset_sim_ball(); resetSim = FALSE; initKF = TRUE; timeSimInit = get_time(); } } display_sim_ball(); return TRUE; }
void charger_task(void) { int next_state; int wait_time = T1_USEC; timestamp_t pre_chg_start = get_time(); pmu_init(); /* Enable low current charging */ pmu_low_current_charging(1); /* Enable charger interrupt */ gpio_enable_interrupt(GPIO_CHARGER_INT_L); /* * EC STOP mode support * The charging loop can be stopped in idle state with AC unplugged. * Charging loop will be resumed by TPSCHROME interrupt. */ enable_charging(0); disable_sleep(SLEEP_MASK_CHARGING); while (1) { last_waken = get_time(); pmu_clear_irq(); #ifdef CONFIG_PMU_TPS65090_CHARGING_LED update_battery_led(); #endif /* * When battery is extremely low, the internal voltage can not * power on its gas guage IC. Charging loop will enable the * charger and turn on trickle charging. For safty reason, * charger should be disabled if the communication to battery * failed. */ if (current_state == ST_PRE_CHARGING && get_time().val - pre_chg_start.val >= PRE_CHARGING_TIMEOUT) next_state = ST_CHARGING_ERROR; else next_state = calc_next_state(current_state); if (next_state != current_state) { /* Reset state of charge moving average window */ rsoc_moving_average(-1); CPRINTS("batt state %s -> %s", state_list[current_state], state_list[next_state]); current_state = next_state; switch (current_state) { case ST_PRE_CHARGING: pre_chg_start = get_time(); /* Fall through */ case ST_CHARGING: if (pmu_blink_led(0)) next_state = ST_CHARGING_ERROR; else enable_charging(1); break; case ST_CHARGING_ERROR: /* * Enable hardware charging circuit after set * PMU to hardware error state. */ if (pmu_blink_led(1)) enable_charging(0); else enable_charging(1); break; case ST_IDLE: case ST_IDLE0: case ST_BAD_COND: case ST_DISCHARGING: enable_charging(0); /* Ignore charger error when discharging */ pmu_blink_led(0); break; } } switch (current_state) { case ST_CHARGING: case ST_CHARGING_ERROR: wait_time = T2_USEC; break; case ST_DISCHARGING: wait_time = T3_USEC; break; case ST_PRE_CHARGING: wait_time = T1_USEC; if (get_time().val - pre_chg_start.val >= PRE_CHARGING_TIMEOUT) enable_charging(0); break; default: if (extpower_is_present()) { wait_time = T1_USEC; break; } else if (chipset_in_state(CHIPSET_STATE_ANY_OFF)) { wait_time = T1_OFF_USEC; enable_sleep(SLEEP_MASK_CHARGING); } else if (chipset_in_state(CHIPSET_STATE_SUSPEND)) { wait_time = T1_SUSPEND_USEC; } else { wait_time = T1_USEC; } } if (!has_pending_event) { task_wait_event(wait_time); disable_sleep(SLEEP_MASK_CHARGING); } else { has_pending_event = 0; } } }