pub fn sqrt_controller(error: f32, p: f32, second_ord_lim: f32, dt: f32) -> f32
Calculate the correction based on a proportional controller (with piecewise sqrt sections to constrain second derivative).