diff --git a/src/lib/wind_estimator/python/derivation.py b/src/lib/wind_estimator/python/derivation.py old mode 100644 new mode 100755 diff --git a/src/lib/wind_estimator/python/derivation_utils.py b/src/lib/wind_estimator/python/derivation_utils.py index 68ee60931e..5bd3efb66f 100644 --- a/src/lib/wind_estimator/python/derivation_utils.py +++ b/src/lib/wind_estimator/python/derivation_utils.py @@ -38,6 +38,8 @@ from symforce import symbolic as sm from symforce import geo from symforce import typing as T +import re + # q: quaternion describing rotation from frame 1 to frame 2 # returns a rotation matrix derived form q which describes the same # rotation @@ -87,6 +89,10 @@ def generate_px4_function(function_name, output_names): line = line.replace("std::min", "math::min") line = line.replace("Eigen", "matrix") line = line.replace("matrix/Dense", "matrix/math.hpp") + + # don't allow underscore + uppercase identifier naming (always reserved for any use) + line = re.sub(r'_([A-Z])', lambda x: '_' + x.group(1).lower(), line) + print(line, end='') def generate_python_function(function_name, output_names): diff --git a/src/lib/wind_estimator/python/generated/fuse_airspeed.h b/src/lib/wind_estimator/python/generated/fuse_airspeed.h index 54b63ffaca..a3d677736d 100644 --- a/src/lib/wind_estimator/python/generated/fuse_airspeed.h +++ b/src/lib/wind_estimator/python/generated/fuse_airspeed.h @@ -58,19 +58,19 @@ void FuseAirspeed(const matrix::Matrix& v_local, // Output terms (4) if (H != nullptr) { - matrix::Matrix& _H = (*H); + matrix::Matrix& _h = (*H); - _H(0, 0) = -_tmp4; - _H(0, 1) = -_tmp5; - _H(0, 2) = _tmp2; + _h(0, 0) = -_tmp4; + _h(0, 1) = -_tmp5; + _h(0, 2) = _tmp2; } if (K != nullptr) { - matrix::Matrix& _K = (*K); + matrix::Matrix& _k = (*K); - _K(0, 0) = _tmp10 * (-P(0, 1) * _tmp5 + P(0, 2) * _tmp2 + _tmp6); - _K(1, 0) = _tmp10 * (-P(1, 0) * _tmp4 + P(1, 2) * _tmp2 + _tmp7); - _K(2, 0) = _tmp10 * (-P(2, 0) * _tmp4 - P(2, 1) * _tmp5 + _tmp8); + _k(0, 0) = _tmp10 * (-P(0, 1) * _tmp5 + P(0, 2) * _tmp2 + _tmp6); + _k(1, 0) = _tmp10 * (-P(1, 0) * _tmp4 + P(1, 2) * _tmp2 + _tmp7); + _k(2, 0) = _tmp10 * (-P(2, 0) * _tmp4 - P(2, 1) * _tmp5 + _tmp8); } if (innov_var != nullptr) { diff --git a/src/lib/wind_estimator/python/generated/fuse_airspeed.py b/src/lib/wind_estimator/python/generated/fuse_airspeed.py index 29971c5ca6..3b6180320f 100644 --- a/src/lib/wind_estimator/python/generated/fuse_airspeed.py +++ b/src/lib/wind_estimator/python/generated/fuse_airspeed.py @@ -43,7 +43,7 @@ def fuse_airspeed(v_local, state, P, airspeed, R, epsilon): # Intermediate terms (11) _tmp0 = -state[0] + v_local[0] _tmp1 = -state[1] + v_local[1] - _tmp2 = math.sqrt(_tmp0**2 + _tmp1**2 + epsilon + v_local[2] ** 2) + _tmp2 = math.sqrt(_tmp0 ** 2 + _tmp1 ** 2 + epsilon + v_local[2] ** 2) _tmp3 = state[2] / _tmp2 _tmp4 = _tmp0 * _tmp3 _tmp5 = _tmp1 * _tmp3 diff --git a/src/lib/wind_estimator/python/generated/fuse_beta.h b/src/lib/wind_estimator/python/generated/fuse_beta.h index 33fd7a0126..39469ce892 100644 --- a/src/lib/wind_estimator/python/generated/fuse_beta.h +++ b/src/lib/wind_estimator/python/generated/fuse_beta.h @@ -70,19 +70,19 @@ void FuseBeta(const matrix::Matrix& v_local, const matrix::Matrix< // Output terms (4) if (H != nullptr) { - matrix::Matrix& _H = (*H); + matrix::Matrix& _h = (*H); - _H(0, 0) = _tmp17; - _H(0, 1) = _tmp18; - _H(0, 2) = 0; + _h(0, 0) = _tmp17; + _h(0, 1) = _tmp18; + _h(0, 2) = 0; } if (K != nullptr) { - matrix::Matrix& _K = (*K); + matrix::Matrix& _k = (*K); - _K(0, 0) = _tmp22 * (P(0, 1) * _tmp18 + _tmp19); - _K(1, 0) = _tmp22 * (P(1, 0) * _tmp17 + _tmp20); - _K(2, 0) = _tmp22 * (P(2, 0) * _tmp17 + P(2, 1) * _tmp18); + _k(0, 0) = _tmp22 * (P(0, 1) * _tmp18 + _tmp19); + _k(1, 0) = _tmp22 * (P(1, 0) * _tmp17 + _tmp20); + _k(2, 0) = _tmp22 * (P(2, 0) * _tmp17 + P(2, 1) * _tmp18); } if (innov_var != nullptr) { diff --git a/src/lib/wind_estimator/python/generated/init_wind_using_airspeed.h b/src/lib/wind_estimator/python/generated/init_wind_using_airspeed.h index 7d53eb5f6c..f79f656009 100644 --- a/src/lib/wind_estimator/python/generated/init_wind_using_airspeed.h +++ b/src/lib/wind_estimator/python/generated/init_wind_using_airspeed.h @@ -56,12 +56,12 @@ void InitWindUsingAirspeed(const matrix::Matrix& v_local, const Sc } if (P != nullptr) { - matrix::Matrix& _P = (*P); + matrix::Matrix& _p = (*P); - _P(0, 0) = _tmp2 * _tmp3 + _tmp4 * airspeed_var + v_var; - _P(1, 0) = _tmp6; - _P(0, 1) = _tmp6; - _P(1, 1) = _tmp2 * airspeed_var + _tmp3 * _tmp4 + v_var; + _p(0, 0) = _tmp2 * _tmp3 + _tmp4 * airspeed_var + v_var; + _p(1, 0) = _tmp6; + _p(0, 1) = _tmp6; + _p(1, 1) = _tmp2 * airspeed_var + _tmp3 * _tmp4 + v_var; } } // NOLINT(readability/fn_size)