static float pid_controller_differentiate(differentiator_t* diff, float input, float dt)
{
	float output = 0.0f;

	if( dt<0.000001f ) 
	{
		output=0.0f;
	} 
	else 
	{
		output = maths_clip(diff->gain * (input - diff->previous) / dt, diff->clip);
	}

	diff->previous = input;
	return output;
}
void simulation_update(simulation_model_t *sim)
{
	int32_t i;
	quat_t qtmp1, qvel_bf,  qed;
	const quat_t front = {.s = 0.0f, .v = {1.0f, 0.0f, 0.0f}};
	const quat_t up = {.s = 0.0f, .v = {UPVECTOR_X, UPVECTOR_Y, UPVECTOR_Z}};
	
	
	uint32_t now = time_keeper_get_micros();
	sim->dt = (now - sim->last_update) / 1000000.0f;
	if (sim->dt > 0.1f)
	{
		sim->dt = 0.1f;
	}
	
	sim->last_update = now;
	// compute torques and forces based on servo commands
	forces_from_servos_diag_quad(sim);
	
	// integrate torques to get simulated gyro rates (with some damping)
	sim->rates_bf[0] = maths_clip((1.0f - 0.1f * sim->dt) * sim->rates_bf[0] + sim->dt * sim->torques_bf[0] / sim->vehicle_config.roll_pitch_momentum, 10.0f);
	sim->rates_bf[1] = maths_clip((1.0f - 0.1f * sim->dt) * sim->rates_bf[1] + sim->dt * sim->torques_bf[1] / sim->vehicle_config.roll_pitch_momentum, 10.0f);
	sim->rates_bf[2] = maths_clip((1.0f - 0.1f * sim->dt) * sim->rates_bf[2] + sim->dt * sim->torques_bf[2] / sim->vehicle_config.yaw_momentum, 10.0f);
	
	
	for (i = 0; i < 3; i++)
	{
			qtmp1.v[i] = 0.5f * sim->rates_bf[i];
	}
	
	qtmp1.s = 0;

	// apply step rotation 
	qed = quaternions_multiply(sim->ahrs.qe,qtmp1);

	// TODO: correct this formulas when using the right scales
	sim->ahrs.qe.s = sim->ahrs.qe.s + qed.s * sim->dt;
	sim->ahrs.qe.v[0] += qed.v[0] * sim->dt;
	sim->ahrs.qe.v[1] += qed.v[1] * sim->dt;
	sim->ahrs.qe.v[2] += qed.v[2] * sim->dt;

	sim->ahrs.qe = quaternions_normalise(sim->ahrs.qe);
	sim->ahrs.up_vec = quaternions_global_to_local(sim->ahrs.qe, up);
	
	sim->ahrs.north_vec = quaternions_global_to_local(sim->ahrs.qe, front);	

	// velocity and position integration
	
	// check altitude - if it is lower than 0, clamp everything (this is in NED, assuming negative altitude)
	if (sim->local_position.pos[Z] >0)
	{
		sim->vel[Z] = 0.0f;
		sim->local_position.pos[Z] = 0.0f;

		// simulate "acceleration" caused by contact force with ground, compensating gravity
		for (i = 0; i < 3; i++)
		{
			sim->lin_forces_bf[i] = sim->ahrs.up_vec.v[i] * sim->vehicle_config.total_mass * sim->vehicle_config.sim_gravity;
		}
				
		// slow down... (will make velocity slightly inconsistent until next update cycle, but shouldn't matter much)
		for (i = 0; i < 3; i++)
		{
			sim->vel_bf[i] = 0.95f * sim->vel_bf[i];
		}
		
		//upright
		sim->rates_bf[0] =  sim->ahrs.up_vec.v[1]; 
		sim->rates_bf[1] =  - sim->ahrs.up_vec.v[0];
		sim->rates_bf[2] = 0;
	}
	
	sim->ahrs.qe = quaternions_normalise(sim->ahrs.qe);
	sim->ahrs.up_vec = quaternions_global_to_local(sim->ahrs.qe, up);
	
	sim->ahrs.north_vec = quaternions_global_to_local(sim->ahrs.qe, front);	
	for (i = 0; i < 3; i++)
	{
			qtmp1.v[i] = sim->vel[i];
	}
	qtmp1.s = 0.0f;
	qvel_bf = quaternions_global_to_local(sim->ahrs.qe, qtmp1);
	
	float acc_bf[3];
	for (i = 0; i < 3; i++)
	{
		sim->vel_bf[i] = qvel_bf.v[i];
		
		// following the convention in the IMU, this is the acceleration due to force, as measured
		sim->ahrs.linear_acc[i] = sim->lin_forces_bf[i] / sim->vehicle_config.total_mass;
		
		// this is the "clean" acceleration without gravity
		acc_bf[i] = sim->ahrs.linear_acc[i] - sim->ahrs.up_vec.v[i] * sim->vehicle_config.sim_gravity;
		
		sim->vel_bf[i] = sim->vel_bf[i] + acc_bf[i] * sim->dt;
	}
	
	// calculate velocity in global frame
	// vel = qe *vel_bf * qe - 1
	qvel_bf.s = 0.0f; qvel_bf.v[0] = sim->vel_bf[0]; qvel_bf.v[1] = sim->vel_bf[1]; qvel_bf.v[2] = sim->vel_bf[2];
	qtmp1 = quaternions_local_to_global(sim->ahrs.qe, qvel_bf);
	sim->vel[0] = qtmp1.v[0]; sim->vel[1] = qtmp1.v[1]; sim->vel[2] = qtmp1.v[2];
	
	for (i = 0; i < 3; i++)
	{
		sim->local_position.pos[i] = sim->local_position.pos[i] + sim->vel[i] * sim->dt;
	}

	// fill in simulated IMU values
	for (i = 0;i < 3; i++)
	{
		sim->imu->raw_gyro.data[sim->imu->calib_gyro.axis[i]] = (sim->rates_bf[i] * sim->calib_gyro.scale_factor[i] + sim->calib_gyro.bias[i]) * sim->calib_gyro.orientation[i];
		sim->imu->raw_accelero.data[sim->imu->calib_accelero.axis[i]] = ((sim->lin_forces_bf[i] / sim->vehicle_config.total_mass / sim->vehicle_config.sim_gravity) * sim->calib_accelero.scale_factor[i] + sim->calib_accelero.bias[i]) * sim->calib_accelero.orientation[i];
		sim->imu->raw_compass.data[sim->imu->calib_compass.axis[i]] = ((sim->ahrs.north_vec.v[i] ) * sim->calib_compass.scale_factor[i] + sim->calib_compass.bias[i])* sim->calib_compass.orientation[i];
	}

	sim->local_position.heading = coord_conventions_get_yaw(sim->ahrs.qe);
}

void simulation_simulate_barometer(simulation_model_t *sim)
{
	sim->pressure->altitude = sim->local_position.origin.altitude - sim->local_position.pos[Z];
	sim->pressure->vario_vz = sim->vel[Z];
	sim->pressure->last_update = time_keeper_get_millis();
	sim->pressure->altitude_offset = 0;
}
	
void simulation_simulate_gps(simulation_model_t *sim)
{
	global_position_t gpos = coord_conventions_local_to_global_position(sim->local_position);
	
	sim->gps->altitude = gpos.altitude;
	sim->gps->latitude = gpos.latitude;
	sim->gps->longitude = gpos.longitude;
	sim->gps->time_last_msg = time_keeper_get_millis();
	sim->gps->time_last_posllh_msg = time_keeper_get_millis();
	sim->gps->time_last_velned_msg = time_keeper_get_millis();

	sim->gps->north_speed = sim->vel[X];
	sim->gps->east_speed = sim->vel[Y];
	sim->gps->vertical_speed = sim->vel[Z];

	sim->gps->status = GPS_OK;
	sim->gps->healthy = true;
}
static float pid_controller_integrate(integrator_t* integrator, float input, float dt)
{
	integrator->accumulator = integrator->accumulator + maths_clip(dt* integrator->gain * input, integrator->clip_pre);
	integrator->accumulator = maths_clip(integrator->accumulator, integrator->clip);
	return integrator->accumulator;
}
void velocity_controller_copter_update(velocity_controller_copter_t* controller)
{
	float velocity_command_global[3];
	float errors[3];
	float thrust_vector[3];
	float thrust_norm;
	float thrust_dir[3];

	// Get the command velocity in global frame
	switch( controller->velocity_command->mode )
	{
		case VELOCITY_COMMAND_MODE_LOCAL:
			get_velocity_command_from_local_to_global( 	controller,
														velocity_command_global );
		break;

		case VELOCITY_COMMAND_MODE_SEMI_LOCAL:
			get_velocity_command_from_semilocal_to_global( 	controller,
															velocity_command_global );
		break;

		case VELOCITY_COMMAND_MODE_GLOBAL:
			velocity_command_global[X] = controller->velocity_command->xyz[X];
			velocity_command_global[Y] = controller->velocity_command->xyz[Y];
			velocity_command_global[Z] = controller->velocity_command->xyz[Z];
		break;
		
		default:
			velocity_command_global[X] = 0.0f;
			velocity_command_global[Y] = 0.0f;
			velocity_command_global[Z] = 0.0f;
		break;
	}

	// Compute errors
	errors[X] = velocity_command_global[X] - controller->pos_est->vel[X];
	errors[Y] = velocity_command_global[Y] - controller->pos_est->vel[Y];
	errors[Z] = velocity_command_global[Z] - controller->pos_est->vel[Z];		// WARNING: it was multiplied by (-1) in stabilisation_copter.c

	// Update PID
	thrust_vector[X] = pid_controller_update_dt( &controller->pid[X], errors[X], controller->ahrs->dt );				// should be multiplied by mass
	thrust_vector[Y] = pid_controller_update_dt( &controller->pid[Y], errors[Y], controller->ahrs->dt );				// should be multiplied by mass
	// thrust_vector[Z] = - GRAVITY + pid_controller_update_dt( &controller->pid[Z], errors[Z], controller->ahrs->dt );	// should be multiplied by mass
	thrust_vector[Z] = pid_controller_update_dt( &controller->pid[Z], errors[Z], controller->ahrs->dt );	// should be multiplied by mass

	// Compute the norm of the thrust that should be applied
	thrust_norm = vectors_norm(thrust_vector);

	// Compute the direction in which thrust should be apply
	thrust_dir[X] = thrust_vector[X] / thrust_norm;
	thrust_dir[Y] = thrust_vector[Y] / thrust_norm;
	thrust_dir[Z] = thrust_vector[Z] / thrust_norm;

	// Map thrust dir to attitude
	controller->attitude_command->mode 		 = ATTITUDE_COMMAND_MODE_RPY;
	controller->attitude_command->rpy[ROLL]  = maths_clip(thrust_vector[Y], 1);
	controller->attitude_command->rpy[PITCH] = - maths_clip(thrust_vector[X], 1);
	controller->attitude_command->rpy[YAW]   = 0.0f;

	// Map PID output to thrust
	// float max_thrust = 30.0f;			// 10 Newton max thrust
	// controller->thrust_command->thrust 	= maths_clip(thrust_norm/max_thrust, 1.0f) * 2.0f - 1; 
	// controller->thrust_command->thrust 	= maths_clip(thrust_norm, 1.0f) * 2.0f - 1; 
	// controller->thrust_command->thrust 	= - 1.0 + 2 * thrust_norm/max_thrust; 

	// // Map PID output to attitude
	// controller->attitude_command->mode 		 = ATTITUDE_COMMAND_MODE_RPY;
	// controller->attitude_command->rpy[ROLL]  = maths_clip(thrust_vector[Y], 1);
	// controller->attitude_command->rpy[PITCH] = - maths_clip(thrust_vector[X], 1);
	// controller->attitude_command->rpy[YAW]   = 0.0f;

	// Map PID output to thrust
	controller->thrust_command->thrust 	= controller->thrust_hover_point - thrust_vector[Z];
}