Signal generator (#22666)

Add option to generate sine chirp signals for fixed-wing system identification

src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp

@@ -620,22 +620,52 @@ void FwAutotuneAttitudeControl::saveGainsToParams()
 
 const Vector3f FwAutotuneAttitudeControl::getIdentificationSignal()
 {
-	if (_steps_counter > _max_steps) {
-		_signal_sign = (_signal_sign == 1) ? 0 : 1;
-		_steps_counter = 0;
 
-		if (_max_steps > 1) {
-			_max_steps--;
 
-		} else {
+	const hrt_abstime now = hrt_absolute_time();
-			_max_steps = 5;
+	const float t = static_cast<float>(now - _state_start_time) * 1e-6f;
+	float signal = 0.0f;
+
+	switch (_param_fw_sysid_signal_type.get()) {
+	case  static_cast<int32_t>(SignalType::kStep): {
+			if (_steps_counter > _max_steps) {
+				_signal_sign = (_signal_sign == 1) ? 0 : 1;
+				_steps_counter = 0;
+
+				if (_max_steps > 1) {
+					_max_steps--;
+
+				} else {
+					_max_steps = 5;
+				}
+			}
+
+			_steps_counter++;
+			signal = float(_signal_sign);
 		}
+		break;
+
+	case static_cast<int32_t>(SignalType::kLinearSineSweep): {
+
+			signal = signal_generator::getLinearSineSweep(_param_fw_at_sysid_f0.get(),
+					_param_fw_at_sysid_f1.get(),
+					_param_fw_sysid_time.get(), t);
+		}
+		break;
+
+	case static_cast<int32_t>(SignalType::kLogSineSweep): {
+			signal = signal_generator::getLogSineSweep(_param_fw_at_sysid_f0.get(), _param_fw_at_sysid_f1.get(),
+					_param_fw_sysid_time.get(), t);
+		}
+		break;
+
+	default:
+		signal = 0.f;
+		break;
 	}
 
-	_steps_counter++;
-
-	const float signal = float(_signal_sign) * _param_fw_at_sysid_amp.get();
 
+	signal *= _param_fw_at_sysid_amp.get();
 	Vector3f rate_sp{};
 
 	float signal_scaled = 0.f;

src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp

@@ -44,6 +44,7 @@
 #include <lib/perf/perf_counter.h>
 #include <lib/pid_design/pid_design.hpp>
 #include <lib/system_identification/system_identification.hpp>
+#include <lib/system_identification/signal_generator.hpp>
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
@@ -64,6 +65,12 @@
 
 using namespace time_literals;
 
+enum class SignalType : uint8_t {
+	kStep = 0,
+	kLinearSineSweep,
+	kLogSineSweep
+};
+
 class FwAutotuneAttitudeControl : public ModuleBase<FwAutotuneAttitudeControl>, public ModuleParams,
 	public px4::WorkItem
 {
@@ -204,7 +211,12 @@ private:
 		(ParamFloat<px4::params::FW_YR_P>) _param_fw_yr_p,
 		(ParamFloat<px4::params::FW_YR_I>) _param_fw_yr_i,
 		(ParamFloat<px4::params::FW_YR_FF>) _param_fw_yr_ff,
-		(ParamFloat<px4::params::FW_Y_RMAX>) _param_fw_y_rmax
+		(ParamFloat<px4::params::FW_Y_RMAX>) _param_fw_y_rmax,
+
+		(ParamFloat<px4::params::FW_AT_SYSID_F0>) _param_fw_at_sysid_f0,
+		(ParamFloat<px4::params::FW_AT_SYSID_F1>) _param_fw_at_sysid_f1,
+		(ParamFloat<px4::params::FW_AT_SYSID_TIME>) _param_fw_sysid_time,
+		(ParamInt<px4::params::FW_AT_SYSID_TYPE>) _param_fw_sysid_signal_type
 	)
 
 	static constexpr float _publishing_dt_s = 100e-3f;

src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control_params.c

@@ -121,3 +121,54 @@ PARAM_DEFINE_INT32(FW_AT_AXES, 3);
  * @group Autotune
  */
 PARAM_DEFINE_INT32(FW_AT_MAN_AUX, 0);
+
+/**
+ * Start frequency of the injected signal
+ *
+ * Can be set lower or higher than the end frequency
+ *
+ * @min 0.1
+ * @max 30.0
+ * @decimal 1
+ * @unit Hz
+ * @group Autotune
+ */
+PARAM_DEFINE_FLOAT(FW_AT_SYSID_F0, 1.f);
+
+/**
+ * End frequency of the injected signal
+ *
+ * Can be set lower or higher than the start frequency
+ *
+ * @min 0.1
+ * @max 30.0
+ * @decimal 1
+ * @unit Hz
+ * @group Autotune
+ */
+PARAM_DEFINE_FLOAT(FW_AT_SYSID_F1, 20.f);
+
+/**
+ * Maneuver time for each axis
+ *
+ * Duration of the input signal sent on each axis during system identification
+ *
+ * @min 5
+ * @max 120
+ * @decimal 0
+ * @unit s
+ * @group Autotune
+ */
+PARAM_DEFINE_FLOAT(FW_AT_SYSID_TIME, 10.f);
+
+/**
+ * Input signal type
+ *
+ * Type of signal used during system identification to excite the system.
+ *
+ * @value 0 Step
+ * @value 1 Linear sine sweep
+ * @value 2 Logarithmic sine sweep
+ * @group Autotune
+ */
+PARAM_DEFINE_INT32(FW_AT_SYSID_TYPE, 0);