Merge branch 'main' into pr-metric-allocator-library

src/lib/mathlib/math/filter/AlphaFilter.hpp

@@ -89,6 +89,16 @@ class AlphaFilter
 		return true;
 	}
 
+	void setCutoffFreq(float cutoff_freq)
+	{
+		if (cutoff_freq > FLT_EPSILON) {
+			_time_constant = 1.f / (M_TWOPI_F * cutoff_freq);
+
+		} else {
+			_time_constant = 0.f;
+		}
+	}
+
 	/**
 	 * Set filter parameter alpha directly without time abstraction
 	 *

src/modules/control_allocator/Kconfig

@@ -10,3 +10,10 @@ menuconfig USER_CONTROL_ALLOCATOR
 	depends on BOARD_PROTECTED && MODULES_CONTROL_ALLOCATOR
 	---help---
 		Put control_allocator in userspace memory
+
+menuconfig CONTROL_ALLOCATOR_RPM_CONTROL
+	bool "Include RPM control for Helicopter rotor"
+	default n
+	depends on MODULES_CONTROL_ALLOCATOR
+	---help---
+		Add support for controlling the helicopter main rotor rpm

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp

@@ -135,11 +135,15 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector<float,
 	_saturation_flags = {};
 
 	const float spoolup_progress = throttleSpoolupProgress();
+	float rpm_control_output = 0;
+#if CONTROL_ALLOCATOR_RPM_CONTROL
 	_rpm_control.setSpoolupProgress(spoolup_progress);
+	rpm_control_output = _rpm_control.getActuatorCorrection();
+#endif // CONTROL_ALLOCATOR_RPM_CONTROL
 
 	// throttle/collective pitch curve
-	const float throttle = (math::interpolateN(-control_sp(ControlAxis::THRUST_Z),
-				_geometry.throttle_curve) + _rpm_control.getActuatorCorrection()) * spoolup_progress;
+	const float throttle = (math::interpolateN(-control_sp(ControlAxis::THRUST_Z), _geometry.throttle_curve)
+				+ rpm_control_output) * spoolup_progress;
 	const float collective_pitch = math::interpolateN(-control_sp(ControlAxis::THRUST_Z), _geometry.pitch_curve);
 
 	// actuator mapping

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp

@@ -134,5 +134,7 @@ class ActuatorEffectivenessHelicopter : public ModuleParams, public ActuatorEffe
 
 	const ActuatorType _tail_actuator_type;
 
-	RpmControl _rpm_control{this};
+#if CONTROL_ALLOCATOR_RPM_CONTROL
+	RpmControl _rpm_control {this};
+#endif // CONTROL_ALLOCATOR_RPM_CONTROL
 };

src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp

@@ -43,7 +43,7 @@
 
 #include "ekf.h"
 #include <ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h>
-#include <ekf_derivation/generated/compute_sideslip_h_and_k.h>
+#include <ekf_derivation/generated/compute_sideslip_h.h>
 
 #include <mathlib/mathlib.h>
 
@@ -127,10 +127,9 @@ bool Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
 	_fault_status.flags.bad_sideslip = false;
 
 	const float epsilon = 1e-3f;
-	VectorState H; // Observation jacobian
-	VectorState K; // Kalman gain vector
 
-	sym::ComputeSideslipHAndK(_state.vector(), P, sideslip.innovation_variance, epsilon, &H, &K);
+	const VectorState H = sym::ComputeSideslipH(_state.vector(), epsilon);
+	VectorState K = P * H / sideslip.innovation_variance;
 
 	if (update_wind_only) {
 		const Vector2f K_wind = K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0);
@@ -143,7 +142,5 @@ bool Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
 	sideslip.fused = true;
 	sideslip.time_last_fuse = _time_delayed_us;
 
-	_fault_status.flags.bad_sideslip = false;
-
 	return true;
 }

src/modules/ekf2/EKF/python/ekf_derivation/derivation.py

@@ -354,10 +354,8 @@ def compute_sideslip_innov_and_innov_var(
 
     return (innov, innov_var)
 
-def compute_sideslip_h_and_k(
+def compute_sideslip_h(
         state: VState,
-        P: MTangent,
-        innov_var: sf.Scalar,
         epsilon: sf.Scalar
 ) -> (VTangent, VTangent):
 
@@ -366,9 +364,7 @@ def compute_sideslip_h_and_k(
 
     H = jacobian_chain_rule(sideslip_pred, state)
 
-    K = P * H.T / sf.Max(innov_var, epsilon)
-
-    return (H.T, K)
+    return (H.T)
 
 def predict_vel_body(
         state: VState
@@ -739,7 +735,7 @@ def compute_gravity_z_innov_var_and_h(
     generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"])
     generate_px4_function(compute_drag_x_innov_var_and_h, output_names=["innov_var", "Hx"])
     generate_px4_function(compute_drag_y_innov_var_and_h, output_names=["innov_var", "Hy"])
-    generate_px4_function(compute_sideslip_h_and_k, output_names=["H", "K"])
+    generate_px4_function(compute_sideslip_h, output_names=None)
     generate_px4_function(compute_sideslip_innov_and_innov_var, output_names=["innov", "innov_var"])
     generate_px4_function(compute_wind_init_and_cov_from_airspeed, output_names=["wind", "P_wind"])
     generate_px4_function(compute_wind_init_and_cov_from_wind_speed_and_direction, output_names=["wind", "P_wind"])

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h.h

@@ -0,0 +1,96 @@
+// -----------------------------------------------------------------------------
+// This file was autogenerated by symforce from template:
+//     function/FUNCTION.h.jinja
+// Do NOT modify by hand.
+// -----------------------------------------------------------------------------
+
+#pragma once
+
+#include <matrix/math.hpp>
+
+namespace sym {
+
+/**
+ * This function was autogenerated from a symbolic function. Do not modify by hand.
+ *
+ * Symbolic function: compute_sideslip_h
+ *
+ * Args:
+ *     state: Matrix25_1
+ *     epsilon: Scalar
+ *
+ * Outputs:
+ *     res: Matrix24_1
+ */
+template <typename Scalar>
+matrix::Matrix<Scalar, 24, 1> ComputeSideslipH(const matrix::Matrix<Scalar, 25, 1>& state,
+                                              const Scalar epsilon) {
+  // Total ops: 131
+
+  // Input arrays
+
+  // Intermediate terms (37)
+  const Scalar _tmp0 = -state(22, 0) + state(4, 0);
+  const Scalar _tmp1 = 2 * state(1, 0);
+  const Scalar _tmp2 = 2 * state(6, 0);
+  const Scalar _tmp3 = _tmp2 * state(3, 0);
+  const Scalar _tmp4 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
+  const Scalar _tmp5 = _tmp4 - 2 * std::pow(state(2, 0), Scalar(2));
+  const Scalar _tmp6 = 2 * state(0, 0);
+  const Scalar _tmp7 = _tmp6 * state(3, 0);
+  const Scalar _tmp8 = 2 * state(2, 0);
+  const Scalar _tmp9 = _tmp8 * state(1, 0);
+  const Scalar _tmp10 = _tmp7 + _tmp9;
+  const Scalar _tmp11 = -state(23, 0) + state(5, 0);
+  const Scalar _tmp12 = _tmp1 * state(3, 0) - _tmp8 * state(0, 0);
+  const Scalar _tmp13 = _tmp0 * _tmp5 + _tmp10 * _tmp11 + _tmp12 * state(6, 0);
+  const Scalar _tmp14 = _tmp13 + epsilon * ((((_tmp13) > 0) - ((_tmp13) < 0)) + Scalar(0.5));
+  const Scalar _tmp15 = Scalar(1.0) / (_tmp14);
+  const Scalar _tmp16 = _tmp2 * state(0, 0);
+  const Scalar _tmp17 = std::pow(_tmp14, Scalar(2));
+  const Scalar _tmp18 = _tmp4 - 2 * std::pow(state(1, 0), Scalar(2));
+  const Scalar _tmp19 = -_tmp7 + _tmp9;
+  const Scalar _tmp20 = _tmp6 * state(1, 0) + _tmp8 * state(3, 0);
+  const Scalar _tmp21 = _tmp0 * _tmp19 + _tmp11 * _tmp18 + _tmp20 * state(6, 0);
+  const Scalar _tmp22 = _tmp21 / _tmp17;
+  const Scalar _tmp23 = _tmp17 / (_tmp17 + std::pow(_tmp21, Scalar(2)));
+  const Scalar _tmp24 = (Scalar(1) / Scalar(2)) * _tmp23;
+  const Scalar _tmp25 = _tmp24 * (_tmp15 * (_tmp0 * _tmp1 + _tmp3) -
+                                  _tmp22 * (-4 * _tmp0 * state(2, 0) + _tmp1 * _tmp11 - _tmp16));
+  const Scalar _tmp26 = 2 * state(3, 0);
+  const Scalar _tmp27 = _tmp2 * state(1, 0);
+  const Scalar _tmp28 = _tmp2 * state(2, 0);
+  const Scalar _tmp29 =
+      _tmp24 * (_tmp15 * (-_tmp0 * _tmp26 + _tmp27) - _tmp22 * (_tmp11 * _tmp26 - _tmp28));
+  const Scalar _tmp30 = _tmp24 * (_tmp15 * (_tmp0 * _tmp8 - 4 * _tmp11 * state(1, 0) + _tmp16) -
+                                  _tmp22 * (_tmp11 * _tmp8 + _tmp3));
+  const Scalar _tmp31 = 4 * state(3, 0);
+  const Scalar _tmp32 = _tmp24 * (_tmp15 * (-_tmp0 * _tmp6 - _tmp11 * _tmp31 + _tmp28) -
+                                  _tmp22 * (-_tmp0 * _tmp31 + _tmp11 * _tmp6 + _tmp27));
+  const Scalar _tmp33 = _tmp22 * _tmp5;
+  const Scalar _tmp34 = _tmp15 * _tmp19;
+  const Scalar _tmp35 = _tmp15 * _tmp18;
+  const Scalar _tmp36 = _tmp10 * _tmp22;
+
+  // Output terms (1)
+  matrix::Matrix<Scalar, 24, 1> _res;
+
+  _res.setZero();
+
+  _res(0, 0) =
+      -_tmp25 * state(3, 0) - _tmp29 * state(1, 0) + _tmp30 * state(0, 0) + _tmp32 * state(2, 0);
+  _res(1, 0) =
+      _tmp25 * state(0, 0) - _tmp29 * state(2, 0) + _tmp30 * state(3, 0) - _tmp32 * state(1, 0);
+  _res(2, 0) =
+      _tmp25 * state(1, 0) - _tmp29 * state(3, 0) - _tmp30 * state(2, 0) + _tmp32 * state(0, 0);
+  _res(3, 0) = _tmp23 * (-_tmp33 + _tmp34);
+  _res(4, 0) = _tmp23 * (_tmp35 - _tmp36);
+  _res(5, 0) = _tmp23 * (-_tmp12 * _tmp22 + _tmp15 * _tmp20);
+  _res(21, 0) = _tmp23 * (_tmp33 - _tmp34);
+  _res(22, 0) = _tmp23 * (-_tmp35 + _tmp36);
+
+  return _res;
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym