dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

lib/wind_estimator: symforce codegen remove reserved identifier naming 

- from the C standard, "All identifiers that begin with an underscore and either an uppercase letter or another underscore are always reserved for any use"
This commit is contained in:
Daniel Agar 2023-01-26 12:55:30 -05:00
parent efe1d43550
commit 47215bb625
6 changed files with 28 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

0
src/lib/wind_estimator/python/derivation.py Normal file → Executable file
View File

6
src/lib/wind_estimator/python/derivation_utils.py
View File

@ -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):

16
src/lib/wind_estimator/python/generated/fuse_airspeed.h
View File

@ -58,19 +58,19 @@ void FuseAirspeed(const matrix::Matrix<Scalar, 3, 1>& v_local,
// Output terms (4)
if (H != nullptr) {
matrix::Matrix<Scalar, 1, 3>& _H = (*H);
matrix::Matrix<Scalar, 1, 3>& _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<Scalar, 3, 1>& _K = (*K);
matrix::Matrix<Scalar, 3, 1>& _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) {

2
src/lib/wind_estimator/python/generated/fuse_airspeed.py
View File

@ -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

16
src/lib/wind_estimator/python/generated/fuse_beta.h
View File

@ -70,19 +70,19 @@ void FuseBeta(const matrix::Matrix<Scalar, 3, 1>& v_local, const matrix::Matrix<
// Output terms (4)
if (H != nullptr) {
matrix::Matrix<Scalar, 1, 3>& _H = (*H);
matrix::Matrix<Scalar, 1, 3>& _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<Scalar, 3, 1>& _K = (*K);
matrix::Matrix<Scalar, 3, 1>& _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) {

10
src/lib/wind_estimator/python/generated/init_wind_using_airspeed.h
View File

@ -56,12 +56,12 @@ void InitWindUsingAirspeed(const matrix::Matrix<Scalar, 3, 1>& v_local, const Sc
}
if (P != nullptr) {
matrix::Matrix<Scalar, 2, 2>& _P = (*P);
matrix::Matrix<Scalar, 2, 2>& _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)