C++ (Cpp) maths_soft_zone Examples

Example #1

File: pid_controller.c Project: fschill/MAVRIC_Library

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

Example #2

File: acoustic.cpp Project: VoidShrine/MAVRIC_Library

void acoustic_set_speed_command(audio_t* audio_data, float rel_pos[], float dist2wpSqr)
{
    float   norm_rel_dist, v_desiredxy, v_desiredz;
    float dir_desired_bf[3], new_velocity[3];

    norm_rel_dist =  rel_pos[2];

    dir_desired_bf[2] = rel_pos[2];

    v_desiredz = maths_f_min(audio_data->navigation->cruise_speed, (audio_data->navigation->dist2vel_gain * maths_soft_zone(norm_rel_dist, audio_data->stabilisation_copter->stabiliser_stack.velocity_stabiliser.thrust_controller.soft_zone_width)));

    if (audio_data->reliabe_el < 1.35)
    {
        v_desiredxy = maths_f_min(audio_data->navigation->cruise_speed, (maths_center_window_2(4.0f * audio_data->reliabe_az) * (-9.0f * audio_data->reliabe_el + (1.35 * 9))));
    }
    else
    {
        v_desiredxy = 0;
    }

    if (v_desiredz *  maths_f_abs(dir_desired_bf[Z]) > audio_data->navigation->max_climb_rate * norm_rel_dist)
    {
        v_desiredz = audio_data->navigation->max_climb_rate * norm_rel_dist / maths_f_abs(dir_desired_bf[Z]);
    }

    dir_desired_bf[X] = v_desiredxy * quick_trig_cos(audio_data->reliabe_az);
    dir_desired_bf[Y] = v_desiredxy * quick_trig_sin(audio_data->reliabe_az);
    dir_desired_bf[Z] = v_desiredz  * dir_desired_bf[Z] / norm_rel_dist;


    for (uint8_t i = 0; i < 3; i++)
    {
        new_velocity[i] = dir_desired_bf[i];
    }

    audio_data->controls_nav->tvel[X]   = new_velocity[X];
    audio_data->controls_nav->tvel[Y]   = new_velocity[Y];
    audio_data->controls_nav->tvel[Z]   = new_velocity[Z];
    audio_data->controls_nav->rpy[YAW]  = KP_YAW * audio_data->reliabe_az;
}