Example #1
0
void imu_update(imu_t *imu)
{
	uint32_t t = time_keeper_get_time_ticks();
	
	imu->dt = time_keeper_ticks_to_seconds(t - imu->last_update);
	imu->last_update = t;

	imu_raw2oriented(imu);
	imu_oriented2scale(imu);
}
Example #2
0
float pid_controller_update(pid_controller_t* controller, float error)
{
	uint32_t t = time_keeper_get_time_ticks();
	controller->error 		= maths_soft_zone(error, controller->soft_zone_width);
	controller->dt 			= time_keeper_ticks_to_seconds(t - controller->last_update);
	controller->last_update = t;
	controller->output 		= controller->p_gain * controller->error  
								+ pid_controller_integrate( &controller->integrator, controller->error, controller->dt)
								+ pid_controller_differentiate(&controller->differentiator, controller->error, controller->dt);
	
	if( controller->output < controller->clip_min ) 
	{
		controller->output = controller->clip_min;
	}

	if( controller->output > controller->clip_max ) 
	{
		controller->output=controller->clip_max;
	}

	return controller->output;	
}
double time_keeper_get_time()
{
	// time in seconds since system start
	return time_keeper_ticks_to_seconds(time_keeper_get_time_ticks());
}