diff --git a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp index 8c6aa020e6c..91853226e9c 100644 --- a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp +++ b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp @@ -493,7 +493,7 @@ bool FlightTaskAuto::_compute_heading_from_2D_vector(float &heading, Vector2f v) // and multiply by the sign given of cross product of x and v. // Dot product: (x(0)*v(0)+(x(1)*v(1)) = v(0) // Cross product: x(0)*v(1) - v(0)*x(1) = v(1) - heading = math::sign(v(1)) * wrap_pi(acosf(v(0))); + heading = sign(v(1)) * wrap_pi(acosf(v(0))); return true; } diff --git a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index c5ff40fc656..6c0c19acb5d 100644 --- a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -175,7 +175,7 @@ float FlightTaskAutoLineSmoothVel::_constrainOneSide(float val, float constraint float FlightTaskAutoLineSmoothVel::_constrainAbs(float val, float max) { - return math::sign(val) * math::min(fabsf(val), fabsf(max)); + return sign(val) * math::min(fabsf(val), fabsf(max)); } float FlightTaskAutoLineSmoothVel::_getMaxXYSpeed() const @@ -301,7 +301,7 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints() else if (z_pos_setpoint_valid) { // Use Z position setpoint to generate a Z velocity setpoint - const float z_dir = math::sign(_position_setpoint(2) - _trajectory[2].getCurrentPosition()); + const float z_dir = sign(_position_setpoint(2) - _trajectory[2].getCurrentPosition()); const float vel_sp_z = z_dir * _getMaxZSpeed(); // If available, use the existing velocity as a feedforward, otherwise replace it diff --git a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp b/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp index adfd681109e..30af1448ab7 100644 --- a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp @@ -117,7 +117,7 @@ bool FlightTaskOrbit::setRadius(float r) // small radius is more important than high velocity for safety if (!checkAcceleration(r, _v, _acceleration_max)) { - _v = math::sign(_v) * sqrtf(_acceleration_max * r); + _v = sign(_v) * sqrtf(_acceleration_max * r); } _r = r; diff --git a/src/lib/flight_tasks/tasks/Utility/VelocitySmoothing.cpp b/src/lib/flight_tasks/tasks/Utility/VelocitySmoothing.cpp index 8ca9e5fb503..7dfd951e2e1 100644 --- a/src/lib/flight_tasks/tasks/Utility/VelocitySmoothing.cpp +++ b/src/lib/flight_tasks/tasks/Utility/VelocitySmoothing.cpp @@ -36,6 +36,9 @@ #include #include #include +#include + +using matrix::sign; VelocitySmoothing::VelocitySmoothing(float initial_accel, float initial_vel, float initial_pos) { @@ -183,12 +186,12 @@ int VelocitySmoothing::computeDirection() float vel_zero_acc = computeVelAtZeroAcc(); /* Depending of the direction, start accelerating positively or negatively */ - int direction = math::sign(_vel_sp - vel_zero_acc); + int direction = sign(_vel_sp - vel_zero_acc); if (direction == 0) { // If by braking immediately the velocity is exactly // the require one with zero acceleration, then brake - direction = math::sign(_state.a); + direction = sign(_state.a); } return direction; @@ -199,7 +202,7 @@ float VelocitySmoothing::computeVelAtZeroAcc() float vel_zero_acc = _state.v; if (fabsf(_state.a) > FLT_EPSILON) { - float j_zero_acc = -math::sign(_state.a) * _max_jerk; // Required jerk to reduce the acceleration + float j_zero_acc = -sign(_state.a) * _max_jerk; // Required jerk to reduce the acceleration float t_zero_acc = -_state.a / j_zero_acc; // Required time to cancel the current acceleration vel_zero_acc = _state.v + _state.a * t_zero_acc + 0.5f * j_zero_acc * t_zero_acc * t_zero_acc; } diff --git a/src/lib/mathlib/math/Functions.hpp b/src/lib/mathlib/math/Functions.hpp index 4aed63d5666..05c4c1f451a 100644 --- a/src/lib/mathlib/math/Functions.hpp +++ b/src/lib/mathlib/math/Functions.hpp @@ -41,16 +41,11 @@ #include "Limits.hpp" +#include + namespace math { -// Type-safe signum function -template -int sign(T val) -{ - return (T(FLT_EPSILON) < val) - (val < T(FLT_EPSILON)); -} - // Type-safe signum function with zero treated as positive template int signNoZero(T val) @@ -114,7 +109,7 @@ const T deadzone(const T &value, const T &dz) T x = constrain(value, (T) - 1, (T) 1); T dzc = constrain(dz, (T) 0, (T) 0.99); // Rescale the input such that we get a piecewise linear function that will be continuous with applied deadzone - T out = (x - sign(x) * dzc) / (1 - dzc); + T out = (x - matrix::sign(x) * dzc) / (1 - dzc); // apply the deadzone (values zero around the middle) return out * (fabsf(x) > dzc); } diff --git a/src/lib/matrix b/src/lib/matrix index 4873dc1c1e7..a32892926c6 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit 4873dc1c1e74f492ec0fcd5b493c9b500f2784fa +Subproject commit a32892926c69ea0bad031fd9d7bfc915cc5e9b68 diff --git a/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp b/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp index 2824635cb6d..53ed68e5a93 100644 --- a/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp +++ b/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp @@ -39,6 +39,8 @@ #include "zero_order_hover_thrust_ekf.hpp" +using matrix::sign; + void ZeroOrderHoverThrustEkf::predict(const float dt) { // State is constant @@ -69,7 +71,7 @@ void ZeroOrderHoverThrustEkf::fuseAccZ(const float acc_z, const float thrust, st bumpStateVariance(); } - const float signed_innov_test_ratio = math::sign(innov) * innov_test_ratio; + const float signed_innov_test_ratio = sign(innov) * innov_test_ratio; updateLpf(residual, signed_innov_test_ratio); updateMeasurementNoise(residual, H); diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index f220b4fe222..16dfbf12fd7 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -219,7 +219,7 @@ void PositionControl::_velocityControl(const float dt) _vel_int += vel_error.emult(_gain_vel_i) * dt; // limit thrust integral - _vel_int(2) = math::min(fabsf(_vel_int(2)), _lim_thr_max) * math::sign(_vel_int(2)); + _vel_int(2) = math::min(fabsf(_vel_int(2)), _lim_thr_max) * sign(_vel_int(2)); } bool PositionControl::_updateSuccessful() diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index 4aff9054988..2f6508817c3 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -47,9 +47,7 @@ #define ACTUATOR_PUBLISH_PERIOD_MS 4 -using matrix::Eulerf; -using matrix::Quatf; -using matrix::Vector3f; +using namespace matrix; /** * L1 control app start / stop handling function @@ -261,7 +259,7 @@ RoverPositionControl::control_position(const matrix::Vector2f ¤t_position, float desired_r = ground_speed_2d.norm_squared() / math::abs_t(_gnd_control.nav_lateral_acceleration_demand()); float desired_theta = (0.5f * M_PI_F) - atan2f(desired_r, _param_wheel_base.get()); - float control_effort = (desired_theta / _param_max_turn_angle.get()) * math::sign( + float control_effort = (desired_theta / _param_max_turn_angle.get()) * sign( _gnd_control.nav_lateral_acceleration_demand()); control_effort = math::constrain(control_effort, -1.0f, 1.0f); _act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort;