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