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