Пример #1
0
void stabilisation_run(stabiliser_t *stabiliser, float dt, float errors[]) 
{
	for (int32_t i = 0; i < 3; i++) 
	{
		stabiliser->output.rpy[i] =	pid_controller_update_dt(&(stabiliser->rpy_controller[i]),  errors[i], dt);
	}		
	stabiliser->output.thrust = pid_controller_update_dt(&(stabiliser->thrust_controller),  errors[3], dt);
}
bool Attitude_controller::update(void)
{
    float now      = time_keeper_get_s();
    dt_s_          = now - last_update_s_;
    last_update_s_ = now;

    // Get attitude command
    attitude_error_estimator_set_quat_ref(&attitude_error_estimator_,
                                          attitude_command_);

    // Get local angular errors
    attitude_error_estimator_update(&attitude_error_estimator_);
    float errors[3];
    errors[ROLL]    = attitude_error_estimator_.rpy_errors[ROLL];
    errors[PITCH]   = attitude_error_estimator_.rpy_errors[PITCH];
    errors[YAW]     = attitude_error_estimator_.rpy_errors[YAW];

    // Update PIDs
    rate_command_.xyz[ROLL]  = pid_controller_update_dt(&pid_[ROLL],  errors[ROLL],  dt_s_);
    rate_command_.xyz[PITCH] = pid_controller_update_dt(&pid_[PITCH], errors[PITCH], dt_s_);
    rate_command_.xyz[YAW]   = pid_controller_update_dt(&pid_[YAW],   errors[YAW],   dt_s_);

    return true;
}
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];
}