diff --git a/conf/airframes/JPG/jpg_cyclone.xml b/conf/airframes/JPG/jpg_cyclone.xml
new file mode 100644
index 0000000000..1c69708fa7
--- /dev/null
+++ b/conf/airframes/JPG/jpg_cyclone.xml
@@ -0,0 +1,283 @@
+
+
+
+ Cyclone 2 airframe
+
+ + Model: Cyclone 2 is a tiltbody wing with a ~1m wingspan
+ + Autopilot: Cube orange
+ + Actuators: Feetech sts3032
+
+ + MAG ΡΜ3100
+ + ESC: KISS ECSs
+ + RCRX: ELRS RP1
+ + AIRSPEED: MS4525DO
+ + TELEMETRY: RFDesign 868X
+ + CURRENT: Standard FC Power sensor
+ + RANGER: None
+ + MOTOR: 2 x 1250KV Outrunner Brushless
+ + PROP: 2 blade thin electric 8 x 6
+ + BAT: Li-Po 4S1P 2300mAh
+
+ WARNING: Current PPRZ airspeed sensor driver has a bug,
+ static and dynamic pressure defaults are swapped,
+ so the airspeed is negative if you dont change your pitot leads
+
+ NOTES:
+ + Using oneloop ANDI
+ + Servos powered via external BEC (4s to 9 V)
+ + Flightcontroller powered via FC Powerboard
+ + Telemetry powerd via BEC of FC Powerboard
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/autopilot/jpg_cyclone_andi.xml b/conf/autopilot/jpg_cyclone_andi.xml
new file mode 100644
index 0000000000..8e334bbb1d
--- /dev/null
+++ b/conf/autopilot/jpg_cyclone_andi.xml
@@ -0,0 +1,194 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/flight_plans/JPG/jpg_cyberzoo_basic.xml b/conf/flight_plans/JPG/jpg_cyberzoo_basic.xml
new file mode 100644
index 0000000000..e9cc25b31b
--- /dev/null
+++ b/conf/flight_plans/JPG/jpg_cyberzoo_basic.xml
@@ -0,0 +1,32 @@
+
+
+
+
+#include "autopilot.h"
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/stabilization_andi.xml b/conf/modules/stabilization_andi.xml
new file mode 100644
index 0000000000..35202b5ba6
--- /dev/null
+++ b/conf/modules/stabilization_andi.xml
@@ -0,0 +1,170 @@
+
+
+
+
+
+ Full ANDI stabilization controller for rotorcraft
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ stabilization_rotorcraft,@attitude_command,wls
+ commands
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/radios/JPG/jpg_elrs.xml b/conf/radios/JPG/jpg_elrs.xml
new file mode 100644
index 0000000000..8aff6de091
--- /dev/null
+++ b/conf/radios/JPG/jpg_elrs.xml
@@ -0,0 +1,27 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/conf/telemetry/JPG/jpg_cyclone.xml b/conf/telemetry/JPG/jpg_cyclone.xml
new file mode 100644
index 0000000000..10d3e555db
--- /dev/null
+++ b/conf/telemetry/JPG/jpg_cyclone.xml
@@ -0,0 +1,252 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/conf/userconf/JPG/jpg_conf.xml b/conf/userconf/JPG/jpg_conf.xml
new file mode 100644
index 0000000000..2fe996e965
--- /dev/null
+++ b/conf/userconf/JPG/jpg_conf.xml
@@ -0,0 +1,13 @@
+
+
+
diff --git a/conf/userconf/JPG/jpg_control_panel.xml b/conf/userconf/JPG/jpg_control_panel.xml
new file mode 100644
index 0000000000..c1da0e6af6
--- /dev/null
+++ b/conf/userconf/JPG/jpg_control_panel.xml
@@ -0,0 +1,208 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/fix_code_style.sh b/fix_code_style.sh
index 2b85552d8c..0355aa1c1b 100755
--- a/fix_code_style.sh
+++ b/fix_code_style.sh
@@ -10,13 +10,39 @@ if [ $# -eq 0 ]; then
exit 1
fi
-ASTYLE_VERSION=`astyle --version 2>&1| awk '{print $4}'`
-ASTYLE_VERSION=${ASTYLE_VERSION:0:4}
+ASTYLE_VERSION=$(astyle --version 2>&1 | awk '{print $4}' | head -1)
echo "Using astyle version $ASTYLE_VERSION"
set -f
-if [ $(bc <<< "$ASTYLE_VERSION >= 2.03") -eq 1 ]; then
+# Safe version parsing with defaults
+major=${ASTYLE_VERSION%%.*}
+minor=$(echo "$ASTYLE_VERSION" | cut -d. -f2)
+
+# Fix unary operator - quote variables and provide defaults
+major=${major:-0}
+minor=${minor:-0}
+
+if [ "$major" -ge 3 ]; then
+ echo "Using AStyle 3.x options"
+ astyle --style=kr \
+ --indent=spaces=2 \
+ --convert-tabs \
+ --indent-switches \
+ --indent-preproc-block \
+ --pad-oper \
+ --pad-header \
+ --unpad-paren \
+ --keep-one-line-blocks \
+ --keep-one-line-statements \
+ --align-pointer=name \
+ --suffix=none \
+ --lineend=linux \
+ --ignore-exclude-errors-x \
+ --max-code-length=120 \
+ "$@"
+elif [ "$major" -ge 2 ]; then
+ echo "Using AStyle 2.x options"
astyle --style=kr \
--indent=spaces=2 \
--convert-tabs \
@@ -33,8 +59,9 @@ if [ $(bc <<< "$ASTYLE_VERSION >= 2.03") -eq 1 ]; then
--add-brackets \
--ignore-exclude-errors-x \
--max-code-length=120 \
- $*
+ "$@"
else
+ echo "Using AStyle 1.x options"
astyle --style=kr \
--indent=spaces=2 \
--convert-tabs \
@@ -49,5 +76,5 @@ else
--suffix=none \
--lineend=linux \
--add-brackets \
- $*
+ "$@"
fi
diff --git a/sw/airborne/filters/complementary_filter.h b/sw/airborne/filters/complementary_filter.h
new file mode 100644
index 0000000000..5c75cf8387
--- /dev/null
+++ b/sw/airborne/filters/complementary_filter.h
@@ -0,0 +1,307 @@
+/*
+ *
+ * Copyright (C) 2025 Justin Dubois
+ *
+ * This file is part of paparazzi
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ *
+ */
+
+/** @file filters/complementary_filter.h
+ * @brief Implementation of complementary filters (first order, second order, and Butterworth variants)
+ *
+ * Provides structures and functions to initialize, update, reset, and get outputs from
+ * first order and second order complementary filters, as well as Butterworth complementary filters.
+ *
+ * FIXME: This implementation of complementary filters forces user to first integrate or differentiate
+ * the signals outside of the filter before passing them in. Consider extending the filter to handle integration
+ * and differentiation internally. Maybe implement cascaded complementary filters for this purpose.
+ *
+ * @author Justin Dubois
+ */
+
+#ifndef COMPLEMENTARY_FILTER_H
+#define COMPLEMENTARY_FILTER_H
+
+#include "std.h"
+#include "filters/low_pass_filter.h"
+
+
+/**
+ * @brief First order complementary filter structure.
+ *
+ * This structure contains two first order low-pass filters,
+ * one for the high-pass path and one for the low-pass path.
+ */
+struct FirstOrderComplementary {
+ struct FirstOrderLowPass x_lp_filter; // Low pass filter instance for high pass path
+ struct FirstOrderLowPass y_lp_filter; // Low pass filter instance for low pass path
+};
+
+/** Initialize the first order complementary filter.
+ *
+ * @param filter Complementary filter struct
+ * @param cut_off Time constant of the low-pass filter
+ * @param sample_time Sampling period
+ * @param value Initial value for filter history
+ */
+static inline void init_first_order_complementary(
+ struct FirstOrderComplementary *filter,
+ float cut_off, float sample_time,
+ float value)
+{
+ init_first_order_low_pass(&filter->x_lp_filter, 1.0f / cut_off, sample_time, value);
+ init_first_order_low_pass(&filter->y_lp_filter, 1.0f / cut_off, sample_time, value);
+}
+
+/** Update the first order complementary filter with new input values.
+ * @param filter Complementary filter struct
+ * @param value_x New input value from the high-pass path
+ * @param value_y New input value from the low-pass path
+ * @return New filtered output value
+ */
+static inline float update_first_order_complementary(
+ struct FirstOrderComplementary *filter,
+ float value_x, float value_y)
+{
+ float x_lp_output = update_first_order_low_pass(&filter->x_lp_filter, value_x);
+ float y_lp_output = update_first_order_low_pass(&filter->y_lp_filter, value_y);
+ return filter->x_lp_filter.last_in - x_lp_output + y_lp_output;
+}
+
+/**
+ * @brief Reset the first order complementary filter to a specific value.
+ *
+ * @param filter Complementary filter struct
+ * @param value Value to reset the filter to
+ * @return The reset value
+ */
+static inline float reset_first_order_complementary(
+ struct FirstOrderComplementary *filter,
+ float value)
+{
+ reset_first_order_low_pass(&filter->x_lp_filter, value);
+ reset_first_order_low_pass(&filter->y_lp_filter, value);
+ return value;
+}
+
+/** Get current value of the first order complementary filter.
+ * @param filter Complementary filter struct
+ * @return Current output value of the filter
+ */
+static inline float get_first_order_complementary(const struct FirstOrderComplementary *filter)
+{
+ float x_lp_output = get_first_order_low_pass(&filter->x_lp_filter);
+ float y_lp_output = get_first_order_low_pass(&filter->y_lp_filter);
+ return filter->x_lp_filter.last_in - x_lp_output + y_lp_output;
+}
+
+/**
+ * @brief Second order complementary filter structure.
+ *
+ * This structure contains two second order low-pass filters,
+ * one for the high-pass path and one for the low-pass path.
+ */
+struct SecondOrderComplementary {
+ struct SecondOrderLowPass x_lp_filter; // Low pass filter instance for high pass path
+ struct SecondOrderLowPass y_lp_filter; // Low pass filter instance for low pass path
+};
+
+/** Initialize the second order complementary filter.
+ *
+ * @param filter Complementary filter struct
+ * @param cut_off Time constant of the low-pass filter
+ * @param Q Q factor of the low-pass filter
+ * @param sample_time Sampling period
+ * @param value Initial value for filter history
+ */
+static inline void init_second_order_complementary(
+ struct SecondOrderComplementary *filter,
+ float cut_off, float Q, float sample_time,
+ float value)
+{
+ init_second_order_low_pass(&filter->x_lp_filter, 1.0f / cut_off, Q, sample_time, value);
+ init_second_order_low_pass(&filter->y_lp_filter, 1.0f / cut_off, Q, sample_time, value);
+}
+
+/** Update the second order complementary filter with new input values.
+ *
+ * @param filter Complementary filter struct
+ * @param value_x New input value from the high-pass path
+ * @param value_y New input value from the low-pass path
+ * @return New filtered output value
+ */
+static inline float update_second_order_complementary(
+ struct SecondOrderComplementary *filter,
+ float value_x, float value_y)
+{
+ float x_lp_output = update_second_order_low_pass(&filter->x_lp_filter, value_x);
+ float y_lp_output = update_second_order_low_pass(&filter->y_lp_filter, value_y);
+ return filter->x_lp_filter.i[0] - x_lp_output + y_lp_output;
+}
+
+/**
+ * @brief Reset the second order complementary filter to a specific value.
+ *
+ * @param filter Complementary filter struct
+ * @param value Value to reset the filter to
+ * @return The reset value
+ */
+static inline float reset_second_order_complementary(
+ struct SecondOrderComplementary *filter,
+ float value)
+{
+ reset_second_order_low_pass(&filter->x_lp_filter, value);
+ reset_second_order_low_pass(&filter->y_lp_filter, value);
+ return value;
+}
+
+/** Get current value of the second order complementary filter.
+ *
+ * @param filter Complementary filter struct
+ * @return Current output value of the filter
+ */
+static inline float get_second_order_complementary(const struct SecondOrderComplementary *filter)
+{
+ float x_lp_output = get_second_order_low_pass(&filter->x_lp_filter);
+ float y_lp_output = get_second_order_low_pass(&filter->y_lp_filter);
+ return filter->x_lp_filter.i[0] - x_lp_output + y_lp_output;
+}
+
+typedef struct SecondOrderComplementary Butterworth2Complementary;
+
+/** Initialize the Butterworth 2nd order low-pass complementary filter.
+ *
+ * @param filter Complementary filter struct
+ * @param cut_off Time constant of the low-pass filter
+ * @param sample_time Sampling period
+ * @param value Initial value for filter history
+ */
+static inline void init_butterworth_2_complementary(
+ Butterworth2Complementary *filter,
+ float cut_off, float sample_time,
+ float value)
+{
+ init_second_order_complementary(
+ (struct SecondOrderComplementary *)filter,
+ cut_off, 0.7071, sample_time,
+ value);
+}
+
+/** Update the Butterworth 2nd order low-pass complementary filter with new input values.
+ *
+ * @param filter Complementary filter struct
+ * @param value_x New input value from the high-pass path
+ * @param value_y New input value from the low-pass path
+ * @return New filtered output value
+ */
+static inline float update_butterworth_2_complementary(Butterworth2Complementary *filter, float value_x, float value_y)
+{
+ return update_second_order_complementary((struct SecondOrderComplementary *)filter, value_x, value_y);
+}
+
+/**
+ * @brief Reset the Butterworth 2nd order complementary filter to a specific value.
+ *
+ * @param filter Complementary filter struct
+ * @param value Value to reset the filter to
+ */
+static inline void reset_butterworth_2_complementary(Butterworth2Complementary *filter, float value)
+{
+ reset_second_order_complementary((struct SecondOrderComplementary *)filter, value);
+}
+
+/** Get current value of the Butterworth 2nd order low-pass complementary filter.
+ *
+ * @param filter Complementary filter struct
+ * @return Current output value of the filter
+ */
+static inline float get_butterworth_2_complementary(const Butterworth2Complementary *filter)
+{
+ return get_second_order_complementary((const struct SecondOrderComplementary *)filter);
+}
+
+/**
+ * @brief 4th order Butterworth complementary filter structure.
+ *
+ * This structure contains two 4th order Butterworth low-pass filters,
+ * one for the high-pass path and one for the low-pass path.
+ */
+typedef struct {
+ Butterworth4LowPass x_lp_filter; // Low pass filter instance for high pass path
+ Butterworth4LowPass y_lp_filter; // Low pass filter instance for low pass path
+} Butterworth4Complementary;
+
+/** Initialize the Butterworth 4th order low-pass complementary filter.
+ *
+ * @param filter Complementary filter struct
+ * @param cut_off Time constant of the low-pass filter
+ * @param sample_time Sampling period
+ * @param value Initial value for filter history
+ */
+static inline void init_butterworth_4_complementary(Butterworth4Complementary *filter, float cut_off, float sample_time,
+ float value)
+{
+ init_butterworth_4_low_pass(&filter->x_lp_filter, 1.0f / cut_off, sample_time, value);
+ init_butterworth_4_low_pass(&filter->y_lp_filter, 1.0f / cut_off, sample_time, value);
+}
+
+/** Update the Butterworth 4th order low-pass complementary filter with new input values.
+ *
+ * @param filter Complementary filter struct
+ * @param value_x New input value from the high-pass path
+ * @param value_y New input value from the low-pass path
+ * @return New filtered output value
+ */
+static inline float update_butterworth_4_complementary(Butterworth4Complementary *filter, float value_x, float value_y)
+{
+ float x_lp_output = update_butterworth_4_low_pass(&filter->x_lp_filter, value_x);
+ float y_lp_output = update_butterworth_4_low_pass(&filter->y_lp_filter, value_y);
+ return filter->x_lp_filter.lp1.i[0] - x_lp_output + y_lp_output;
+}
+
+/**
+ * @brief Reset the Butterworth 4th order complementary filter to a specific value.
+ *
+ * @param filter Complementary filter struct
+ * @param value Value to reset the filter to
+ */
+static inline void reset_butterworth_4_complementary(Butterworth4Complementary *filter, float value)
+{
+ reset_butterworth_4_low_pass(&filter->x_lp_filter, value);
+ reset_butterworth_4_low_pass(&filter->y_lp_filter, value);
+}
+
+/** Get current value of the Butterworth 4th order low-pass complementary filter.
+ *
+ * @param filter Complementary filter struct
+ * @return Current output value of the filter
+ */
+static inline float get_butterworth_4_complementary(const Butterworth4Complementary *filter)
+{
+ float x_lp_output = get_butterworth_4_low_pass(&filter->x_lp_filter);
+ float y_lp_output = get_butterworth_4_low_pass(&filter->y_lp_filter);
+ return filter->x_lp_filter.lp1.i[0] - x_lp_output + y_lp_output;
+}
+
+
+
+
+
+
+
+
+#endif /* COMPLEMENTARY_FILTER_H */
\ No newline at end of file
diff --git a/sw/airborne/filters/complementary_filter_types.h b/sw/airborne/filters/complementary_filter_types.h
new file mode 100644
index 0000000000..aeffb72d76
--- /dev/null
+++ b/sw/airborne/filters/complementary_filter_types.h
@@ -0,0 +1,582 @@
+/*
+ *
+ * Copyright (C) 2025 Justin Dubois
+ *
+ * This file is part of paparazzi
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ *
+ */
+
+/** @file filters/complementary_filter_types.h
+ * @brief Definitions and inline functions for 1st order complementary filter vector types
+ *
+ * Provides structures and functions to initialize, update, reset, and get outputs from
+ * 1st order complementary filter vectors.
+ *
+ * @author Justin Dubois
+ */
+
+#ifndef COMPLEMENTARY_FILTER_TYPES_H
+#define COMPLEMENTARY_FILTER_TYPES_H
+
+#include "std.h"
+#include "filters/complementary_filter.h"
+#include "math/pprz_algebra_float.h"
+
+/**
+ * @brief 3D vector of 1st order complementary filters.
+ *
+ * This structure contains three 1st order complementary filters
+ * for the x, y, and z components.
+ */
+struct FirstOrderComplementaryVect3 {
+ struct FirstOrderComplementary x;
+ struct FirstOrderComplementary y;
+ struct FirstOrderComplementary z;
+};
+
+/**
+ * @brief Initialize 3D vector of 1st order complementary filters.
+ *
+ * @param[out] filter Pointer to the FirstOrderComplementaryVect3 struct.
+ * @param[in] cut_off Time constant in seconds.
+ * @param[in] sample_time Sampling period in seconds.
+ */
+static inline void init_first_order_complementary_vect3(struct FirstOrderComplementaryVect3 *filter,
+ const struct FloatVect3 *cut_off, float sample_time)
+{
+ init_first_order_complementary(&filter->x, cut_off->x, sample_time, 0.0f);
+ init_first_order_complementary(&filter->y, cut_off->y, sample_time, 0.0f);
+ init_first_order_complementary(&filter->z, cut_off->z, sample_time, 0.0f);
+}
+
+/**
+ * @brief Get current output 3D vector from 1st order complementary filters.
+ *
+ * @param[in] filter Pointer to FirstOrderComplementaryVect3 struct.
+ * @return FloatVect3 containing filtered outputs.
+ */
+static inline struct FloatVect3 get_first_order_complementary_vect3(const struct FirstOrderComplementaryVect3 *filter)
+{
+ struct FloatVect3 output;
+ output.x = get_first_order_complementary(&filter->x);
+ output.y = get_first_order_complementary(&filter->y);
+ output.z = get_first_order_complementary(&filter->z);
+ return output;
+}
+
+/**
+ * @brief Get current output rates from 1st order complementary filters.
+ *
+ * @param[in] filter Pointer to FirstOrderComplementaryVect3 struct.
+ * @return FloatRates containing filtered outputs p, q, r.
+ */
+static inline struct FloatRates get_first_order_complementary_rates(const struct FirstOrderComplementaryVect3 *filter)
+{
+ struct FloatRates output;
+ output.p = get_first_order_complementary(&filter->x);
+ output.q = get_first_order_complementary(&filter->y);
+ output.r = get_first_order_complementary(&filter->z);
+ return output;
+}
+
+/**
+ * @brief Update 3D vector of 1st order complementary filters.
+ *
+ * @param[in,out] filter Pointer to the FirstOrderComplementaryVect3 struct.
+ * @param[in] value_x Pointer to FloatVect3 input vector x values.
+ * @param[in] value_y Pointer to FloatVect3 input vector y values.
+ */
+static inline struct FloatVect3 update_first_order_complementary_vect3(struct FirstOrderComplementaryVect3 *filter,
+ const struct FloatVect3 *value_x, const struct FloatVect3 *value_y)
+{
+ update_first_order_complementary(&filter->x, value_x->x, value_y->x);
+ update_first_order_complementary(&filter->y, value_x->y, value_y->y);
+ update_first_order_complementary(&filter->z, value_x->z, value_y->z);
+ return get_first_order_complementary_vect3(filter);
+}
+
+/**
+ * @brief Update 3D vector of 1st order complementary filters for rates.
+ *
+ * @param[in,out] filter Pointer to the FirstOrderComplementaryVect3 struct.
+ * @param[in] value_x Pointer to FloatRates input p, q, r values.
+ * @param[in] value_y Pointer to FloatRates input p, q, r values.
+ */
+static inline struct FloatRates update_first_order_complementary_rates(struct FirstOrderComplementaryVect3 *filter,
+ const struct FloatRates *value_x, const struct FloatRates *value_y)
+{
+ update_first_order_complementary(&filter->x, value_x->p, value_y->p);
+ update_first_order_complementary(&filter->y, value_x->q, value_y->q);
+ update_first_order_complementary(&filter->z, value_x->r, value_y->r);
+ return get_first_order_complementary_rates(filter);
+}
+
+/**
+ * @brief Reset 3D vector of 1st order complementary filters to a specific value.
+ *
+ * @param[in,out] filter Pointer to the FirstOrderComplementaryVect3 struct.
+ * @param[in] value Value to reset the filters to.
+ */
+static inline void reset_first_order_complementary_vect3(struct FirstOrderComplementaryVect3 *filter,
+ const struct FloatVect3 *value)
+{
+ reset_first_order_complementary(&filter->x, value->x);
+ reset_first_order_complementary(&filter->y, value->y);
+ reset_first_order_complementary(&filter->z, value->z);
+}
+
+/**
+ * @brief Reset 3D vector of 1st order complementary filters for rates to a specific value.
+ *
+ * @param[in,out] filter Pointer to the FirstOrderComplementaryVect3 struct.
+ * @param[in] value Value to reset the filters to.
+ */
+static inline void reset_first_order_complementary_rates(struct FirstOrderComplementaryVect3 *filter,
+ const struct FloatRates *value)
+{
+ reset_first_order_complementary(&filter->x, value->p);
+ reset_first_order_complementary(&filter->y, value->q);
+ reset_first_order_complementary(&filter->z, value->r);
+}
+
+/**
+ * @brief Initialize an array of 1st order complementary filters.
+ *
+ * @param[in] n Number of filters in the array.
+ * @param[out] filter_array Array of FirstOrderComplementary filters.
+ * @param[in] cut_off Time constant in seconds.
+ * @param[in] sample_time Sampling period in seconds.
+ */
+static inline void init_first_order_complementary_array(uint8_t n,
+ struct FirstOrderComplementary filter_array[restrict n], const float cut_off[restrict n], float sample_time)
+{
+ for (uint8_t i = 0; i < n; i++) {
+ init_first_order_complementary(&filter_array[i], cut_off[i], sample_time, 0.0f);
+ }
+}
+
+/**
+ * @brief Update an array of 1st order complementary filters.
+ *
+ * @param[in] n Number of filters in the array.
+ * @param[in,out] filter_array Array of FirstOrderComplementary filters.
+ * @param[in] value_x_array Array of input x values.
+ * @param[in] value_y_array Array of input y values.
+ */
+static inline void update_first_order_complementary_array(uint8_t n,
+ struct FirstOrderComplementary filter_array[restrict n], const float value_x_array[restrict n],
+ const float value_y_array[restrict n])
+{
+ for (uint8_t i = 0; i < n; i++) {
+ update_first_order_complementary(&filter_array[i], value_x_array[i], value_y_array[i]);
+ }
+}
+
+/**
+ * @brief Reset an array of 1st order complementary filters to specific values.
+ *
+ * @param[in] n Number of filters in the array.
+ * @param[in,out] filter_array Array of FirstOrderComplementary filters.
+ * @param[in] value_array Array of values to reset the filters to.
+ */
+static inline void reset_first_order_complementary_array(uint8_t n,
+ struct FirstOrderComplementary filter_array[restrict n], const float value_array[restrict n])
+{
+ for (uint8_t i = 0; i < n; i++) {
+ reset_first_order_complementary(&filter_array[i], value_array[i]);
+ }
+}
+
+/**
+ * @brief Get current outputs from an array of 1st order complementary filters.
+ *
+ * @param[in] n Number of filters in the array.
+ * @param[in] filter_array Array of FirstOrderComplementary filters.
+ * @param[out] output_array Array to store the filtered output values.
+ */
+static inline void get_first_order_complementary_array(uint8_t n,
+ const struct FirstOrderComplementary filter_array[restrict n], float output_array[restrict n])
+{
+ for (uint8_t i = 0; i < n; i++) {
+ output_array[i] = get_first_order_complementary(&filter_array[i]);
+ }
+}
+
+/**
+ * @brief 3D vector of 2nd order Butterworth complementary filters.
+ *
+ * This structure contains three 2nd order Butterworth complementary filters
+ * for the x, y, and z components.
+ */
+struct Butterworth2ComplementaryVect3 {
+ Butterworth2Complementary x;
+ Butterworth2Complementary y;
+ Butterworth2Complementary z;
+};
+
+/**
+ * @brief Initialize 3D vector of 2nd order Butterworth complementary filters.
+ *
+ * @param[out] filter Pointer to the Butterworth2ComplementaryVect3 struct.
+ * @param[in] cut_off Time constant in seconds.
+ * @param[in] sample_time Sampling period in seconds.
+ */
+static inline void init_butterworth_2_complementary_vect3(struct Butterworth2ComplementaryVect3 *filter,
+ const struct FloatVect3 *cut_off, float sample_time)
+{
+ init_butterworth_2_complementary(&filter->x, cut_off->x, sample_time, 0.0f);
+ init_butterworth_2_complementary(&filter->y, cut_off->y, sample_time, 0.0f);
+ init_butterworth_2_complementary(&filter->z, cut_off->z, sample_time, 0.0f);
+}
+
+/**
+ * @brief Get current output 3D vector from 2nd order Butterworth complementary filters.
+ *
+ * @param[in] filter Pointer to Butterworth2ComplementaryVect3 struct.
+ * @return FloatVect3 containing filtered outputs.
+ */
+static inline struct FloatVect3 get_butterworth_2_complementary_vect3(const struct Butterworth2ComplementaryVect3
+ *filter)
+{
+ struct FloatVect3 output;
+ output.x = get_butterworth_2_complementary(&filter->x);
+ output.y = get_butterworth_2_complementary(&filter->y);
+ output.z = get_butterworth_2_complementary(&filter->z);
+ return output;
+}
+
+/**
+ * @brief Get current output rates from 2nd order Butterworth complementary filters.
+ *
+ * @param[in] filter Pointer to Butterworth2ComplementaryVect3 struct.
+ * @return FloatRates containing filtered outputs p, q, r.
+ */
+static inline struct FloatRates get_butterworth_2_complementary_rates(const struct Butterworth2ComplementaryVect3
+ *filter)
+{
+ struct FloatRates output;
+ output.p = get_butterworth_2_complementary(&filter->x);
+ output.q = get_butterworth_2_complementary(&filter->y);
+ output.r = get_butterworth_2_complementary(&filter->z);
+ return output;
+}
+
+/**
+ * @brief Initialize 3D vector of 2nd order Butterworth complementary filters.
+ *
+ * @param[out] filter Pointer to the Butterworth2ComplementaryVect3 struct.
+ * @param[in] cut_off Time constant in seconds.
+ * @param[in] sample_time Sampling period in seconds.
+ */
+static inline struct FloatVect3 update_butterworth_2_complementary_vect3(struct Butterworth2ComplementaryVect3 *filter,
+ const struct FloatVect3 *value_x, const struct FloatVect3 *value_y)
+{
+ update_butterworth_2_complementary(&filter->x, value_x->x, value_y->x);
+ update_butterworth_2_complementary(&filter->y, value_x->y, value_y->y);
+ update_butterworth_2_complementary(&filter->z, value_x->z, value_y->z);
+ return get_butterworth_2_complementary_vect3(filter);
+}
+
+/**
+ * @brief Update 3D vector of 2nd order Butterworth complementary filters.
+ *
+ * @param[in,out] filter Pointer to the Butterworth2ComplementaryVect3 struct.
+ * @param[in] value_x Pointer to FloatVect3 input vector x values.
+ * @param[in] value_y Pointer to FloatVect3 input vector y values.
+ */
+static inline struct FloatRates update_butterworth_2_complementary_rates(struct Butterworth2ComplementaryVect3 *filter,
+ const struct FloatRates *value_x, const struct FloatRates *value_y)
+{
+ update_butterworth_2_complementary(&filter->x, value_x->p, value_y->p);
+ update_butterworth_2_complementary(&filter->y, value_x->q, value_y->q);
+ update_butterworth_2_complementary(&filter->z, value_x->r, value_y->r);
+ return get_butterworth_2_complementary_rates(filter);
+}
+
+/**
+ * @brief Reset 3D vector of 2nd order Butterworth complementary filters to a specific value.
+ *
+ * @param[in,out] filter Pointer to the Butterworth2ComplementaryVect3 struct.
+ * @param[in] value Value to reset the filters to.
+ */
+static inline void reset_butterworth_2_complementary_vect3(struct Butterworth2ComplementaryVect3 *filter,
+ const struct FloatVect3 *value)
+{
+ reset_butterworth_2_complementary(&filter->x, value->x);
+ reset_butterworth_2_complementary(&filter->y, value->y);
+ reset_butterworth_2_complementary(&filter->z, value->z);
+}
+
+/**
+ * @brief Reset 3D vector of 2nd order Butterworth complementary filters for rates to a specific value.
+ *
+ * @param[in,out] filter Pointer to the Butterworth2ComplementaryVect3 struct.
+ * @param[in] value Value to reset the filters to.
+ */
+static inline void reset_butterworth_2_complementary_rates(struct Butterworth2ComplementaryVect3 *filter,
+ const struct FloatRates *value)
+{
+ reset_butterworth_2_complementary(&filter->x, value->p);
+ reset_butterworth_2_complementary(&filter->y, value->q);
+ reset_butterworth_2_complementary(&filter->z, value->r);
+}
+
+/**
+ * @brief Initialize an array of 2nd order Butterworth complementary filters.
+ *
+ * @param[in] n Number of filters in the array.
+ * @param[out] filter_array Array of Butterworth2Complementary filters.
+ * @param[in] cut_off Time constant in seconds.
+ * @param[in] sample_time Sampling period in seconds.
+ */
+static inline void init_butterworth_2_complementary_array(uint8_t n, Butterworth2Complementary filter_array[restrict n],
+ const float cut_off[restrict n], float sample_time)
+{
+ for (uint8_t i = 0; i < n; i++) {
+ init_butterworth_2_complementary(&filter_array[i], cut_off[i], sample_time, 0.0f);
+ }
+}
+
+/**
+ * @brief Update an array of 2nd order Butterworth complementary filters.
+ *
+ * @param[in] n Number of filters in the array.
+ * @param[in,out] filter_array Array of Butterworth2Complementary filters.
+ * @param[in] value_x_array Array of input x values.
+ * @param[in] value_y_array Array of input y values.
+ */
+static inline void update_butterworth_2_complementary_array(uint8_t n,
+ Butterworth2Complementary filter_array[restrict n], const float value_x_array[restrict n],
+ const float value_y_array[restrict n])
+{
+ for (uint8_t i = 0; i < n; i++) {
+ update_butterworth_2_complementary(&filter_array[i], value_x_array[i], value_y_array[i]);
+ }
+}
+
+/**
+ * @brief Reset an array of 2nd order Butterworth complementary filters to specific values.
+ *
+ * @param[in] n Number of filters in the array.
+ * @param[in,out] filter_array Array of Butterworth2Complementary filters.
+ * @param[in] value_array Array of values to reset the filters to.
+ */
+static inline void reset_butterworth_2_complementary_array(uint8_t n,
+ Butterworth2Complementary filter_array[restrict n], const float value_array[restrict n])
+{
+ for (uint8_t i = 0; i < n; i++) {
+ reset_butterworth_2_complementary(&filter_array[i], value_array[i]);
+ }
+}
+/**
+ * @brief Get current outputs from an array of 2nd order Butterworth complementary filters.
+ *
+ * @param[in] n Number of filters in the array.
+ * @param[in] filter_array Array of Butterworth2Complementary filters.
+ * @param[out] output_array Array to store the filtered output values.
+ */
+static inline void get_butterworth_2_complementary_array(uint8_t n,
+ const Butterworth2Complementary filter_array[restrict n], float output_array[restrict n])
+{
+ for (uint8_t i = 0; i < n; i++) {
+ output_array[i] = get_butterworth_2_complementary(&filter_array[i]);
+ }
+}
+
+/**
+ * @brief 3D vector of 4th order Butterworth complementary filters.
+ *
+ * This structure contains three 4th order Butterworth complementary filters
+ * for the x, y, and z components.
+ */
+struct Butterworth4ComplementaryVect3 {
+ Butterworth4Complementary x;
+ Butterworth4Complementary y;
+ Butterworth4Complementary z;
+};
+
+/**
+ * @brief Initialize 3D vector of 4th order Butterworth complementary filters.
+ *
+ * @param[out] filter Pointer to the Butterworth4ComplementaryVect3 struct.
+ * @param[in] cut_off Time constant in seconds.
+ * @param[in] sample_time Sampling period in seconds.
+ */
+static inline void init_butterworth_4_complementary_vect3(struct Butterworth4ComplementaryVect3 *filter,
+ const struct FloatVect3 *cut_off, float sample_time)
+{
+ init_butterworth_4_complementary(&filter->x, cut_off->x, sample_time, 0.0f);
+ init_butterworth_4_complementary(&filter->y, cut_off->y, sample_time, 0.0f);
+ init_butterworth_4_complementary(&filter->z, cut_off->z, sample_time, 0.0f);
+}
+
+/**
+ * @brief Get current output 3D vector from 4th order Butterworth complementary filters.
+ *
+ * @param[in] filter Pointer to Butterworth4ComplementaryVect3 struct.
+ * @return FloatVect3 containing filtered outputs.
+ */
+static inline struct FloatVect3 get_butterworth_4_complementary_vect3(const struct Butterworth4ComplementaryVect3
+ *filter)
+{
+ struct FloatVect3 output;
+ output.x = get_butterworth_4_complementary(&filter->x);
+ output.y = get_butterworth_4_complementary(&filter->y);
+ output.z = get_butterworth_4_complementary(&filter->z);
+ return output;
+}
+
+/**
+ * @brief Get current output rates from 4th order Butterworth complementary filters.
+ *
+ * @param[in] filter Pointer to Butterworth4ComplementaryVect3 struct.
+ * @return FloatRates containing filtered outputs p, q, r.
+ */
+static inline struct FloatRates get_butterworth_4_complementary_rates(const struct Butterworth4ComplementaryVect3
+ *filter)
+{
+ struct FloatRates output;
+ output.p = get_butterworth_4_complementary(&filter->x);
+ output.q = get_butterworth_4_complementary(&filter->y);
+ output.r = get_butterworth_4_complementary(&filter->z);
+ return output;
+}
+
+/**
+ * @brief Initialize 3D vector of 4th order Butterworth complementary filters.
+ *
+ * @param[out] filter Pointer to the Butterworth4ComplementaryVect3 struct.
+ * @param[in] cut_off Time constant in seconds.
+ * @param[in] sample_time Sampling period in seconds.
+ */
+static inline struct FloatVect3 update_butterworth_4_complementary_vect3(struct Butterworth4ComplementaryVect3 *filter,
+ const struct FloatVect3 *value_x, const struct FloatVect3 *value_y)
+{
+ update_butterworth_4_complementary(&filter->x, value_x->x, value_y->x);
+ update_butterworth_4_complementary(&filter->y, value_x->y, value_y->y);
+ update_butterworth_4_complementary(&filter->z, value_x->z, value_y->z);
+ return get_butterworth_4_complementary_vect3(filter);
+}
+
+/**
+ * @brief Update 3D vector of 4th order Butterworth complementary filters for rates.
+ *
+ * @param[in,out] filter Pointer to the Butterworth4ComplementaryVect3 struct.
+ * @param[in] value_x Pointer to FloatRates input p, q, r values.
+ * @param[in] value_y Pointer to FloatRates input p, q, r values.
+ */
+static inline struct FloatRates update_butterworth_4_complementary_rates(struct Butterworth4ComplementaryVect3 *filter,
+ const struct FloatRates *value_x, const struct FloatRates *value_y)
+{
+ update_butterworth_4_complementary(&filter->x, value_x->p, value_y->p);
+ update_butterworth_4_complementary(&filter->y, value_x->q, value_y->q);
+ update_butterworth_4_complementary(&filter->z, value_x->r, value_y->r);
+ return get_butterworth_4_complementary_rates(filter);
+}
+
+/**
+ * @brief Reset 3D vector of 4th order Butterworth complementary filters to a specific value.
+ *
+ * @param[in,out] filter Pointer to the Butterworth4ComplementaryVect3 struct.
+ * @param[in] value Value to reset the filters to.
+ */
+static inline void reset_butterworth_4_complementary_vect3(struct Butterworth4ComplementaryVect3 *filter,
+ const struct FloatVect3 *value)
+{
+ reset_butterworth_4_complementary(&filter->x, value->x);
+ reset_butterworth_4_complementary(&filter->y, value->y);
+ reset_butterworth_4_complementary(&filter->z, value->z);
+}
+
+/**
+ * @brief Reset 3D vector of 4th order Butterworth complementary filters for rates to a specific value.
+ *
+ * @param[in,out] filter Pointer to the Butterworth4ComplementaryVect3 struct.
+ * @param[in] value Value to reset the filters to.
+ */
+static inline void reset_butterworth_4_complementary_rates(struct Butterworth4ComplementaryVect3 *filter,
+ const struct FloatRates *value)
+{
+ reset_butterworth_4_complementary(&filter->x, value->p);
+ reset_butterworth_4_complementary(&filter->y, value->q);
+ reset_butterworth_4_complementary(&filter->z, value->r);
+}
+
+/**
+ * @brief Initialize an array of 4th order Butterworth complementary filters.
+ *
+ * @param[in] n Number of filters to initialize.
+ * @param[out] filter_array Array of Butterworth4Complementary filters.
+ * @param[in] cut_off Time constant in seconds.
+ * @param[in] sample_time Sampling period (seconds).
+ */
+static inline void init_butterworth_4_complementary_array(uint8_t n, Butterworth4Complementary filter_array[restrict n],
+ const float cut_off[restrict n], float sample_time)
+{
+ for (uint8_t i = 0; i < n; i++) {
+ init_butterworth_4_complementary(&filter_array[i], cut_off[i], sample_time, 0.0f);
+ }
+}
+
+/**
+ * @brief Update an array of 4th order Butterworth complementary filters.
+ *
+ * @param[in] n Number of filters in the array.
+ * @param[in,out] filter_array Array of Butterworth4Complementary filters.
+ * @param[in] value_x_array Array of input x values.
+ * @param[in] value_y_array Array of input y values.
+ */
+static inline void update_butterworth_4_complementary_array(uint8_t n,
+ Butterworth4Complementary filter_array[restrict n], const float value_x_array[restrict n],
+ const float value_y_array[restrict n])
+{
+ for (uint8_t i = 0; i < n; i++) {
+ update_butterworth_4_complementary(&filter_array[i], value_x_array[i], value_y_array[i]);
+ }
+}
+
+/**
+ * @brief Reset an array of 4th order Butterworth complementary filters to specific values.
+ *
+ * @param[in] n Number of filters in the array.
+ * @param[in,out] filter_array Array of Butterworth4Complementary filters.
+ * @param[in] value_array Array of values to reset the filters to.
+ */
+static inline void reset_butterworth_4_complementary_array(uint8_t n,
+ Butterworth4Complementary filter_array[restrict n], const float value_array[restrict n])
+{
+ for (uint8_t i = 0; i < n; i++) {
+ reset_butterworth_4_complementary(&filter_array[i], value_array[i]);
+ }
+}
+
+/**
+ * @brief Get current outputs from an array of 4th order Butterworth complementary filters.
+ *
+ * @param[in] n Number of filters in the array.
+ * @param[in] filter_array Array of Butterworth4Complementary filters.
+ * @param[out] output_array Array to store the filtered output values.
+ */
+static inline void get_butterworth_4_complementary_array(uint8_t n,
+ const Butterworth4Complementary filter_array[restrict n], float output_array[restrict n])
+{
+ for (uint8_t i = 0; i < n; i++) {
+ output_array[i] = get_butterworth_4_complementary(&filter_array[i]);
+ }
+}
+
+#endif /* COMPLEMENTARY_FILTER_TYPES_H */
diff --git a/sw/airborne/filters/low_pass_filter.h b/sw/airborne/filters/low_pass_filter.h
index 565e74cea6..17dd8b3326 100644
--- a/sw/airborne/filters/low_pass_filter.h
+++ b/sw/airborne/filters/low_pass_filter.h
@@ -22,6 +22,9 @@
/** @file filters/low_pass_filter.h
* @brief Simple first order low pass filter with bilinear transform
*
+ * @FIXME: INT and FLOAT implementations are inconsistent, tau and cut_off freq are mixed.
+ * Suggestion: Change everything to freq.
+ *
*/
#ifndef LOW_PASS_FILTER_H
@@ -54,7 +57,7 @@ struct FirstOrderLowPass {
* @param sample_time sampling period of the signal
* @param value initial value of the filter
*/
-static inline void init_first_order_low_pass(struct FirstOrderLowPass *filter, float tau, float sample_time,
+static inline void init_first_order_low_pass(struct FirstOrderLowPass *filter, float tau, const float sample_time,
float value)
{
filter->last_in = value;
@@ -68,7 +71,7 @@ static inline void init_first_order_low_pass(struct FirstOrderLowPass *filter, f
* @param value new input value of the filter
* @return new filtered value
*/
-static inline float update_first_order_low_pass(struct FirstOrderLowPass *filter, float value)
+static inline float update_first_order_low_pass(struct FirstOrderLowPass *filter, const float value)
{
float out = (value + filter->last_in + (filter->time_const - 1.0f) * filter->last_out) / (1.0f + filter->time_const);
filter->last_in = value;
@@ -76,12 +79,26 @@ static inline float update_first_order_low_pass(struct FirstOrderLowPass *filter
return out;
}
+/**
+ * @brief Reset the first order low-pass filter to a specific value.
+ *
+ * @param filter first order low pass filter structure
+ * @param value Value to reset the filter to
+ * @return The reset value
+ */
+static inline float reset_first_order_low_pass(struct FirstOrderLowPass *filter, const float value)
+{
+ filter->last_in = value;
+ filter->last_out = value;
+ return value;
+}
+
/** Get current value of the first order low pass filter.
*
* @param filter first order low pass filter structure
* @return current value of the filter
*/
-static inline float get_first_order_low_pass(struct FirstOrderLowPass *filter)
+static inline float get_first_order_low_pass(const struct FirstOrderLowPass *filter)
{
return filter->last_out;
}
@@ -92,7 +109,8 @@ static inline float get_first_order_low_pass(struct FirstOrderLowPass *filter)
* @param tau time constant of the first order low pass filter
* @param sample_time sampling period of the signal
*/
-static inline void update_first_order_low_pass_tau(struct FirstOrderLowPass *filter, float tau, float sample_time)
+static inline void update_first_order_low_pass_tau(struct FirstOrderLowPass *filter, const float tau,
+ const float sample_time)
{
filter->time_const = 2.0f * tau / sample_time;
}
@@ -149,7 +167,8 @@ struct SecondOrderLowPass {
* @param sample_time sampling period of the signal
* @param value initial value of the filter
*/
-static inline void init_second_order_low_pass(struct SecondOrderLowPass *filter, float tau, float Q, float sample_time,
+static inline void init_second_order_low_pass(struct SecondOrderLowPass *filter, const float tau, const float Q,
+ const float sample_time,
float value)
{
float K = tanf(sample_time / (2.0f * tau));
@@ -161,13 +180,26 @@ static inline void init_second_order_low_pass(struct SecondOrderLowPass *filter,
filter->i[0] = filter->i[1] = filter->o[0] = filter->o[1] = value;
}
+/**
+ * @brief Reset the second order low-pass filter to a specific value.
+ *
+ * @param filter second order low pass filter structure
+ * @param value Value to reset the filter to
+ * @return The reset value
+ */
+static inline float reset_second_order_low_pass(struct SecondOrderLowPass *filter, const float value)
+{
+ filter->i[0] = filter->i[1] = filter->o[0] = filter->o[1] = value;
+ return value;
+}
+
/** Update second order low pass filter state with a new value.
*
* @param filter second order low pass filter structure
* @param value new input value of the filter
* @return new filtered value
*/
-static inline float update_second_order_low_pass(struct SecondOrderLowPass *filter, float value)
+static inline float update_second_order_low_pass(struct SecondOrderLowPass *filter, const float value)
{
float out = filter->b[0] * value
+ filter->b[1] * filter->i[0]
@@ -186,7 +218,7 @@ static inline float update_second_order_low_pass(struct SecondOrderLowPass *filt
* @param filter second order low pass filter structure
* @return current value of the filter
*/
-static inline float get_second_order_low_pass(struct SecondOrderLowPass *filter)
+static inline float get_second_order_low_pass(const struct SecondOrderLowPass *filter)
{
return filter->o[0];
}
@@ -207,7 +239,8 @@ struct SecondOrderLowPass_int {
* @param sample_time sampling period of the signal
* @param value initial value of the filter
*/
-static inline void init_second_order_low_pass_int(struct SecondOrderLowPass_int *filter, float cut_off, float Q,
+static inline void init_second_order_low_pass_int(struct SecondOrderLowPass_int *filter, const float cut_off,
+ const float Q,
float sample_time, int32_t value)
{
struct SecondOrderLowPass filter_temp;
@@ -236,7 +269,7 @@ static inline void init_second_order_low_pass_int(struct SecondOrderLowPass_int
* @param value new input value of the filter
* @return new filtered value
*/
-static inline int32_t update_second_order_low_pass_int(struct SecondOrderLowPass_int *filter, int32_t value)
+static inline int32_t update_second_order_low_pass_int(struct SecondOrderLowPass_int *filter, const int32_t value)
{
int32_t out = filter->b[0] * value
+ filter->b[1] * filter->i[0]
@@ -256,7 +289,7 @@ static inline int32_t update_second_order_low_pass_int(struct SecondOrderLowPass
* @param filter second order low pass filter structure
* @return current value of the filter
*/
-static inline int32_t get_second_order_low_pass_int(struct SecondOrderLowPass_int *filter)
+static inline int32_t get_second_order_low_pass_int(const struct SecondOrderLowPass_int *filter)
{
return filter->o[0];
}
@@ -277,7 +310,8 @@ typedef struct SecondOrderLowPass Butterworth2LowPass;
* @param sample_time sampling period of the signal
* @param value initial value of the filter
*/
-static inline void init_butterworth_2_low_pass(Butterworth2LowPass *filter, float tau, float sample_time, float value)
+static inline void init_butterworth_2_low_pass(Butterworth2LowPass *filter, const float tau, const float sample_time,
+ const float value)
{
init_second_order_low_pass((struct SecondOrderLowPass *)filter, tau, 0.7071, sample_time, value);
}
@@ -288,19 +322,31 @@ static inline void init_butterworth_2_low_pass(Butterworth2LowPass *filter, floa
* @param value new input value of the filter
* @return new filtered value
*/
-static inline float update_butterworth_2_low_pass(Butterworth2LowPass *filter, float value)
+static inline float update_butterworth_2_low_pass(Butterworth2LowPass *filter, const float value)
{
return update_second_order_low_pass((struct SecondOrderLowPass *)filter, value);
}
+/**
+ * @brief Reset a Butterworth low-pass filter to a specific value.
+ *
+ * @param[out] filter Butterworth2LowPass filter instance to reset.
+ * @param[in] value Value to reset the filter to.
+ * @return The reset value.
+ */
+static inline float reset_butterworth_2_low_pass(Butterworth2LowPass *filter, const float value)
+{
+ return reset_second_order_low_pass((struct SecondOrderLowPass *)filter, value);
+}
+
/** Get current value of the second order Butterworth low pass filter.
*
* @param filter second order Butterworth low pass filter structure
* @return current value of the filter
*/
-static inline float get_butterworth_2_low_pass(Butterworth2LowPass *filter)
+static inline float get_butterworth_2_low_pass(const Butterworth2LowPass *filter)
{
- return filter->o[0];
+ return get_second_order_low_pass((const struct SecondOrderLowPass *)filter);
}
/** Second order Butterworth low pass filter(fixed point version).
@@ -319,7 +365,8 @@ typedef struct SecondOrderLowPass_int Butterworth2LowPass_int;
* @param sample_time sampling period of the signal
* @param value initial value of the filter
*/
-static inline void init_butterworth_2_low_pass_int(Butterworth2LowPass_int *filter, float cut_off, float sample_time,
+static inline void init_butterworth_2_low_pass_int(Butterworth2LowPass_int *filter, const float cut_off,
+ const float sample_time,
int32_t value)
{
init_second_order_low_pass_int((struct SecondOrderLowPass_int *)filter, cut_off, 0.7071, sample_time, value);
@@ -331,7 +378,7 @@ static inline void init_butterworth_2_low_pass_int(Butterworth2LowPass_int *filt
* @param value new input value of the filter
* @return new filtered value
*/
-static inline int32_t update_butterworth_2_low_pass_int(Butterworth2LowPass_int *filter, int32_t value)
+static inline int32_t update_butterworth_2_low_pass_int(Butterworth2LowPass_int *filter, const int32_t value)
{
return update_second_order_low_pass_int((struct SecondOrderLowPass_int *)filter, value);
}
@@ -341,7 +388,7 @@ static inline int32_t update_butterworth_2_low_pass_int(Butterworth2LowPass_int
* @param filter second order Butterworth low pass filter structure
* @return current value of the filter
*/
-static inline int32_t get_butterworth_2_low_pass_int(Butterworth2LowPass_int *filter)
+static inline int32_t get_butterworth_2_low_pass_int(const Butterworth2LowPass_int *filter)
{
return filter->o[0];
}
@@ -368,7 +415,8 @@ typedef struct {
* @param sample_time sampling period of the signal
* @param value initial value of the filter
*/
-static inline void init_butterworth_4_low_pass(Butterworth4LowPass *filter, float tau, float sample_time, float value)
+static inline void init_butterworth_4_low_pass(Butterworth4LowPass *filter, const float tau, const float sample_time,
+ const float value)
{
init_second_order_low_pass(&filter->lp1, tau, 1.30651, sample_time, value);
init_second_order_low_pass(&filter->lp2, tau, 0.51184, sample_time, value);
@@ -382,7 +430,7 @@ static inline void init_butterworth_4_low_pass(Butterworth4LowPass *filter, floa
* @param value new input value of the filter
* @return new filtered value
*/
-static inline float update_butterworth_4_low_pass(Butterworth4LowPass *filter, float value)
+static inline float update_butterworth_4_low_pass(Butterworth4LowPass *filter, const float value)
{
float tmp = update_second_order_low_pass(&filter->lp1, value);
return update_second_order_low_pass(&filter->lp2, tmp);
@@ -393,11 +441,23 @@ static inline float update_butterworth_4_low_pass(Butterworth4LowPass *filter, f
* @param filter fourth order Butterworth low pass filter structure
* @return current value of the filter
*/
-static inline float get_butterworth_4_low_pass(Butterworth4LowPass *filter)
+static inline float get_butterworth_4_low_pass(const Butterworth4LowPass *filter)
{
return filter->lp2.o[0];
}
+/**
+ * @brief Reset a Butterworth low-pass filter to a specific value.
+ *
+ * @param[out] filter Butterworth4LowPass filter instance to reset.
+ * @param[in] value Value to reset the filter to.
+ */
+static inline void reset_butterworth_4_low_pass(Butterworth4LowPass *filter, const float value)
+{
+ filter->lp1.i[0] = filter->lp1.i[1] = filter->lp1.o[0] = filter->lp1.o[1] = filter->lp2.i[0] = filter->lp2.i[1] =
+ filter->lp2.o[0] = filter->lp2.o[1] = value;
+}
+
/** Fourth order Butterworth low pass filter(fixed point version).
*
* using two cascaded second order filters
@@ -420,7 +480,8 @@ typedef struct {
* @param sample_time sampling period of the signal
* @param value initial value of the filter
*/
-static inline void init_butterworth_4_low_pass_int(Butterworth4LowPass_int *filter, float cut_off, float sample_time,
+static inline void init_butterworth_4_low_pass_int(Butterworth4LowPass_int *filter, const float cut_off,
+ const float sample_time,
int32_t value)
{
init_second_order_low_pass_int(&filter->lp1, cut_off, 1.30651, sample_time, value);
@@ -435,7 +496,7 @@ static inline void init_butterworth_4_low_pass_int(Butterworth4LowPass_int *filt
* @param value new input value of the filter
* @return new filtered value
*/
-static inline int32_t update_butterworth_4_low_pass_int(Butterworth4LowPass_int *filter, int32_t value)
+static inline int32_t update_butterworth_4_low_pass_int(Butterworth4LowPass_int *filter, const int32_t value)
{
int32_t tmp = update_second_order_low_pass_int(&filter->lp1, value);
return update_second_order_low_pass_int(&filter->lp2, tmp);
@@ -446,7 +507,7 @@ static inline int32_t update_butterworth_4_low_pass_int(Butterworth4LowPass_int
* @param filter fourth order Butterworth low pass filter structure
* @return current value of the filter
*/
-static inline int32_t get_butterworth_4_low_pass_int(Butterworth4LowPass_int *filter)
+static inline int32_t get_butterworth_4_low_pass_int(const Butterworth4LowPass_int *filter)
{
return filter->lp2.o[0];
}
diff --git a/sw/airborne/filters/low_pass_filter_types.h b/sw/airborne/filters/low_pass_filter_types.h
new file mode 100644
index 0000000000..e065eab6ac
--- /dev/null
+++ b/sw/airborne/filters/low_pass_filter_types.h
@@ -0,0 +1,550 @@
+/*
+ *
+ * Copyright (C) 2025 Justin Dubois
+ *
+ * This file is part of paparazzi
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ *
+ */
+
+/** @file filters/low_pass_filter_types.h
+ * @brief Definitions and inline functions for 1st order low-pass filter vector types
+ *
+ * Provides structures and functions to initialize, update, reset, and get outputs from
+ * 1st order low-pass filter vectors.
+ *
+ * @author Justin Dubois
+ */
+
+#ifndef LOW_PASS_FILTER_TYPES_H
+#define LOW_PASS_FILTER_TYPES_H
+
+#include "std.h"
+#include "filters/low_pass_filter.h"
+#include "math/pprz_algebra_float.h"
+
+struct FirstOrderLowPassVect3 {
+ struct FirstOrderLowPass x;
+ struct FirstOrderLowPass y;
+ struct FirstOrderLowPass z;
+};
+
+/**
+ * @brief Initialize a set of first order low-pass filters to zero for 3D vector data.
+ *
+ * @param[out] filter Struct containing first order low-pass filters for x, y, z components.
+ * @param[in] cut_off Time constant for the filters (s).
+ * @param[in] dt Sampling time interval (seconds).
+ */
+static inline void init_first_order_low_pass_vect3(struct FirstOrderLowPassVect3 *filter,
+ const struct FloatVect3 *cut_off, const float dt)
+{
+ init_first_order_low_pass(&filter->x, 1.0f / cut_off->x, dt, 0.0f);
+ init_first_order_low_pass(&filter->y, 1.0f / cut_off->y, dt, 0.0f);
+ init_first_order_low_pass(&filter->z, 1.0f / cut_off->z, dt, 0.0f);
+}
+
+/**
+ * @brief Retrieve the filtered output from 3D vector first order low-pass filters.
+ *
+ * @param[in] filter Struct containing first order low-pass filters for x, y, z components.
+ * @return FloatVect3 struct containing the filtered output values.
+ */
+static inline struct FloatVect3 get_first_order_low_pass_vect3(const struct FirstOrderLowPassVect3 *filter)
+{
+ struct FloatVect3 output;
+ output.x = get_first_order_low_pass(&filter->x);
+ output.y = get_first_order_low_pass(&filter->y);
+ output.z = get_first_order_low_pass(&filter->z);
+ return output;
+}
+
+/**
+ * @brief Retrieve the filtered output from 3D vector first order low-pass filters.
+ *
+ * @param[in] filter Struct containing first order low-pass filters for x, y, z components.
+ * @return FloatRates struct containing the filtered output rate values.
+ */
+static inline struct FloatRates get_first_order_low_pass_rates(const struct FirstOrderLowPassVect3 *filter)
+{
+ struct FloatRates output;
+ output.p = get_first_order_low_pass(&filter->x);
+ output.q = get_first_order_low_pass(&filter->y);
+ output.r = get_first_order_low_pass(&filter->z);
+ return output;
+}
+
+/**
+ * @brief Update 3D vector first order low-pass filters with new input data.
+ *
+ * @param[in,out] filter Struct containing first order low-pass filters for x, y, z components.
+ * @param[in] input Pointer to FloatVect3 struct containing new input data.
+ */
+static inline struct FloatVect3 update_first_order_low_pass_vect3(struct FirstOrderLowPassVect3 *filter,
+ const struct FloatVect3 *input)
+{
+ update_first_order_low_pass(&filter->x, input->x);
+ update_first_order_low_pass(&filter->y, input->y);
+ update_first_order_low_pass(&filter->z, input->z);
+ return get_first_order_low_pass_vect3(filter);
+}
+
+/**
+ * @brief Update 3D vector first order low-pass filters with new rate input data.
+ *
+ * @param[in,out] filter Struct containing first order low-pass filters for x, y, z components.
+ * @param[in] input Pointer to FloatRates struct containing new input rate data.
+ */
+static inline struct FloatRates update_first_order_low_pass_rates(struct FirstOrderLowPassVect3 *filter,
+ const struct FloatRates *input)
+{
+ update_first_order_low_pass(&filter->x, input->p);
+ update_first_order_low_pass(&filter->y, input->q);
+ update_first_order_low_pass(&filter->z, input->r);
+ return get_first_order_low_pass_rates(filter);
+}
+
+/**
+ * @brief Reset 3D vector first order low-pass filters to a specific value.
+ *
+ * @param[out] filter Struct containing first order low-pass filters for x, y, z components.
+ * @param[in] value Pointer to FloatVect3 struct containing the reset value.
+ */
+static inline void reset_first_order_low_pass_vect3(struct FirstOrderLowPassVect3 *filter,
+ const struct FloatVect3 *value)
+{
+ reset_first_order_low_pass(&filter->x, value->x);
+ reset_first_order_low_pass(&filter->y, value->y);
+ reset_first_order_low_pass(&filter->z, value->z);
+}
+
+/**
+ * @brief Reset 3D vector first order low-pass filters to specific rate values.
+ *
+ * @param[out] filter Struct containing first order low-pass filters for x, y, z components.
+ * @param[in] value Pointer to FloatRates struct containing the reset values.
+ */
+static inline void reset_first_order_low_pass_rates(struct FirstOrderLowPassVect3 *filter,
+ const struct FloatRates *value)
+{
+ reset_first_order_low_pass(&filter->x, value->p);
+ reset_first_order_low_pass(&filter->y, value->q);
+ reset_first_order_low_pass(&filter->z, value->r);
+}
+
+/**
+ * @brief Initialize an array of first order low-pass filters.
+ *
+ * @param n Number of filters in the array.
+ * @param filter_array Array of FirstOrderLowPass filters to initialize.
+ * @param cut_off Time constant for the filters (s).
+ * @param dt Sampling time interval (seconds).
+ */
+static inline void init_first_order_low_pass_array(const uint8_t n, struct FirstOrderLowPass filter_array[restrict n],
+ const float cut_off[restrict n], const float dt)
+{
+ for (uint8_t i = 0; i < n; i++) {
+ init_first_order_low_pass(&filter_array[i], 1.0f / cut_off[i], dt, 0.0f);
+ }
+}
+
+/**
+ * @brief Update an array of first order low-pass filters.
+ *
+ * @param n Number of filters in the array.
+ * @param filter_array Array of FirstOrderLowPass filters to update.
+ * @param input_array Array of input values for the filters.
+ */
+static inline void update_first_order_low_pass_array(const uint8_t n, struct FirstOrderLowPass filter_array[restrict n],
+ const float input_array[restrict n])
+{
+ for (uint8_t i = 0; i < n; i++) {
+ update_first_order_low_pass(&filter_array[i], input_array[i]);
+ }
+}
+
+/**
+ * @brief Reset an array of first order low-pass filters to specific values.
+ *
+ * @param n Number of filters in the array.
+ * @param filter_array Array of FirstOrderLowPass filters to reset.
+ * @param value_array Array of values to reset the filters to.
+ */
+static inline void reset_first_order_low_pass_array(const uint8_t n, struct FirstOrderLowPass filter_array[restrict n],
+ const float value_array[restrict n])
+{
+ for (uint8_t i = 0; i < n; i++) {
+ reset_first_order_low_pass(&filter_array[i], value_array[i]);
+ }
+}
+
+/**
+ * @brief Retrieve the filtered outputs from an array of first order low-pass filters.
+ *
+ * @param n Number of filters in the array.
+ * @param filter_array Array of FirstOrderLowPass filters.
+ * @param output_array Array to store the filtered output values.
+ */
+static inline void get_first_order_low_pass_array(const uint8_t n,
+ const struct FirstOrderLowPass filter_array[restrict n], float output_array[restrict n])
+{
+ for (uint8_t i = 0; i < n; i++) {
+ output_array[i] = get_first_order_low_pass(&filter_array[i]);
+ }
+}
+
+
+struct Butterworth2LowPassVect3 {
+ Butterworth2LowPass x;
+ Butterworth2LowPass y;
+ Butterworth2LowPass z;
+};
+
+/**
+ * @brief Initialize a set of Butterworth low-pass filters to zero for 3D vector data.
+ *
+ * @param[out] filter Struct containing Butterworth filters for x, y, z components.
+ * @param[in] cut_off Time constant for the filters (s).
+ * @param[in] dt Sampling time interval (seconds).
+ */
+static inline void init_butterworth_2_low_pass_vect3(struct Butterworth2LowPassVect3 *filter,
+ const struct FloatVect3 *cut_off, const float dt)
+{
+ init_butterworth_2_low_pass(&filter->x, 1.0f / cut_off->x, dt, 0.0f);
+ init_butterworth_2_low_pass(&filter->y, 1.0f / cut_off->y, dt, 0.0f);
+ init_butterworth_2_low_pass(&filter->z, 1.0f / cut_off->z, dt, 0.0f);
+}
+
+/**
+ * @brief Retrieve the filtered output from 3D vector Butterworth filters.
+ *
+ * @param[in] filter Struct containing Butterworth filters for x, y, z components.
+ * @return FloatVect3 struct containing the filtered output values.
+ */
+static inline struct FloatVect3 get_butterworth_2_low_pass_vect3(const struct Butterworth2LowPassVect3 *filter)
+{
+ struct FloatVect3 output;
+ output.x = get_butterworth_2_low_pass(&filter->x);
+ output.y = get_butterworth_2_low_pass(&filter->y);
+ output.z = get_butterworth_2_low_pass(&filter->z);
+ return output;
+}
+
+/**
+ * @brief Retrieve the filtered output from a Butterworth low-pass filter.
+ *
+ * @param[in] filter Butterworth2LowPass filter instance.
+ * @return Filtered output value.
+ */
+static inline struct FloatRates get_butterworth_2_low_pass_rates(const struct Butterworth2LowPassVect3 *filter)
+{
+ struct FloatRates output;
+ output.p = get_butterworth_2_low_pass(&filter->x);
+ output.q = get_butterworth_2_low_pass(&filter->y);
+ output.r = get_butterworth_2_low_pass(&filter->z);
+ return output;
+}
+
+/**
+ * @brief Update 3D vector Butterworth filters with new input data.
+ *
+ * @param[in,out] filter Struct containing Butterworth filters for x, y, z components.
+ * @param[in] input Pointer to FloatVect3 struct containing new input data.
+ */
+static inline struct FloatVect3 update_butterworth_2_low_pass_vect3(struct Butterworth2LowPassVect3 *filter,
+ const struct FloatVect3 *input)
+{
+ update_butterworth_2_low_pass(&filter->x, input->x);
+ update_butterworth_2_low_pass(&filter->y, input->y);
+ update_butterworth_2_low_pass(&filter->z, input->z);
+ return get_butterworth_2_low_pass_vect3(filter);
+}
+
+/**
+ * @brief Update 3D vector Butterworth filters with new rate input data.
+ *
+ * @param[in,out] filter Struct containing Butterworth filters for x, y, z components.
+ * @param[in] input Pointer to FloatRates struct containing new input rate data.
+ */
+static inline struct FloatRates update_butterworth_2_low_pass_rates(struct Butterworth2LowPassVect3 *filter,
+ const struct FloatRates *input)
+{
+ update_butterworth_2_low_pass(&filter->x, input->p);
+ update_butterworth_2_low_pass(&filter->y, input->q);
+ update_butterworth_2_low_pass(&filter->z, input->r);
+ return get_butterworth_2_low_pass_rates(filter);
+}
+
+/**
+ * @brief Reset 3D vector Butterworth filters to a specific value.
+ *
+ * @param[out] filter Struct containing Butterworth filters for x, y, z components.
+ * @param[in] value Pointer to FloatVect3 struct containing the reset value.
+ */
+static inline void reset_butterworth_2_low_pass_vect3(struct Butterworth2LowPassVect3 *filter,
+ const struct FloatVect3 *value)
+{
+ reset_butterworth_2_low_pass(&filter->x, value->x);
+ reset_butterworth_2_low_pass(&filter->y, value->y);
+ reset_butterworth_2_low_pass(&filter->z, value->z);
+}
+
+/**
+ * @brief Reset 3D vector Butterworth filters to specific rate values.
+ *
+ * @param[out] filter Struct containing Butterworth filters for x, y, z components.
+ * @param[in] value Pointer to FloatRates struct containing the reset values.
+ */
+static inline void reset_butterworth_2_low_pass_rates(struct Butterworth2LowPassVect3 *filter,
+ const struct FloatRates *value)
+{
+ reset_butterworth_2_low_pass(&filter->x, value->p);
+ reset_butterworth_2_low_pass(&filter->y, value->q);
+ reset_butterworth_2_low_pass(&filter->z, value->r);
+}
+
+/**
+ * @brief Initialize an array of Butterworth low-pass filters to zero.
+ *
+ * @param[in] n Number of filters to initialize.
+ * @param[out] filter_array Array of Butterworth2LowPass filters to initialize.
+ * @param[in] cut_off Time constant for the filters (s).
+ * @param[in] dt Sampling time interval (seconds).
+ */
+static inline void init_butterworth_2_low_pass_array(const uint8_t n, Butterworth2LowPass filter_array[restrict n],
+ const float cut_off[restrict n], const float dt)
+{
+ for (uint8_t i = 0; i < n; i++) {
+ init_butterworth_2_low_pass(&filter_array[i], 1.0f / cut_off[i], dt, 0.0f);
+ }
+}
+
+/**
+ * @brief Update an array of Butterworth low-pass filters with new input data.
+ *
+ * @param[in] n Number of filters in the array.
+ * @param[in,out] filter_array Array of Butterworth2LowPass filters to update.
+ * @param[in] input_array Array containing new input data for each filter.
+ */
+static inline void update_butterworth_2_low_pass_array(const uint8_t n, Butterworth2LowPass filter_array[restrict n],
+ const float input_array[restrict n])
+{
+ for (uint8_t i = 0; i < n; i++) {
+ update_butterworth_2_low_pass(&filter_array[i], input_array[i]);
+ }
+}
+
+/**
+ * @brief Reset an array of Butterworth low-pass filters to specific values.
+ *
+ * @param[in] n Number of filters in the array.
+ * @param[out] filter_array Array of Butterworth2LowPass filters to reset.
+ * @param[in] value_array Array containing reset values for each filter.
+ */
+static inline void reset_butterworth_2_low_pass_array(const uint8_t n, Butterworth2LowPass filter_array[restrict n],
+ const float value_array[restrict n])
+{
+ for (uint8_t i = 0; i < n; i++) {
+ reset_butterworth_2_low_pass(&filter_array[i], value_array[i]);
+ }
+}
+
+/**
+ * @brief Retrieve the filtered outputs from an array of Butterworth low-pass filters.
+ *
+ * @param[in] n Number of filters in the array.
+ * @param[in] filter_array Array of Butterworth2LowPass filters.
+ * @param[out] output_array Array to store the filtered output values.
+ */
+static inline void get_butterworth_2_low_pass_array(const uint8_t n, const Butterworth2LowPass filter_array[restrict n],
+ float output_array[restrict n])
+{
+ for (uint8_t i = 0; i < n; i++) {
+ output_array[i] = get_butterworth_2_low_pass(&filter_array[i]);
+ }
+}
+
+struct Butterworth4LowPassVect3 {
+ Butterworth4LowPass x;
+ Butterworth4LowPass y;
+ Butterworth4LowPass z;
+};
+
+/**
+ * @brief Initialize a set of Butterworth low-pass filters to zero for 3D vector data.
+ *
+ * @param[out] filter Struct containing Butterworth filters for x, y, z components.
+ * @param[in] cut_off Time constant for the filters (s).
+ * @param[in] dt Sampling time interval (seconds).
+ */
+static inline void init_butterworth_4_low_pass_vect3(struct Butterworth4LowPassVect3 *filter,
+ const struct FloatVect3 *cut_off, const float dt)
+{
+ init_butterworth_4_low_pass(&filter->x, 1.0f / cut_off->x, dt, 0.0f);
+ init_butterworth_4_low_pass(&filter->y, 1.0f / cut_off->y, dt, 0.0f);
+ init_butterworth_4_low_pass(&filter->z, 1.0f / cut_off->z, dt, 0.0f);
+}
+
+/**
+ * @brief Retrieve the filtered output from 3D vector Butterworth filters.
+ *
+ * @param[in] filter Struct containing Butterworth filters for x, y, z components.
+ * @return FloatVect3 struct containing the filtered output values.
+ */
+static inline struct FloatVect3 get_butterworth_4_low_pass_vect3(const struct Butterworth4LowPassVect3 *filter)
+{
+ struct FloatVect3 output;
+ output.x = get_butterworth_4_low_pass(&filter->x);
+ output.y = get_butterworth_4_low_pass(&filter->y);
+ output.z = get_butterworth_4_low_pass(&filter->z);
+ return output;
+}
+
+/**
+ * @brief Retrieve the filtered output from a Butterworth low-pass filter.
+ *
+ * @param[in] filter Butterworth4LowPass filter instance.
+ * @return Filtered output value.
+ */
+static inline struct FloatRates get_butterworth_4_low_pass_rates(const struct Butterworth4LowPassVect3 *filter)
+{
+ struct FloatRates output;
+ output.p = get_butterworth_4_low_pass(&filter->x);
+ output.q = get_butterworth_4_low_pass(&filter->y);
+ output.r = get_butterworth_4_low_pass(&filter->z);
+ return output;
+}
+
+/**
+ * @brief Update 3D vector Butterworth filters with new input data.
+ *
+ * @param[in,out] filter Struct containing Butterworth filters for x, y, z components.
+ * @param[in] input Pointer to FloatVect3 struct containing new input data.
+ */
+static inline struct FloatVect3 update_butterworth_4_low_pass_vect3(struct Butterworth4LowPassVect3 *filter,
+ const struct FloatVect3 *input)
+{
+ update_butterworth_4_low_pass(&filter->x, input->x);
+ update_butterworth_4_low_pass(&filter->y, input->y);
+ update_butterworth_4_low_pass(&filter->z, input->z);
+ return get_butterworth_4_low_pass_vect3(filter);
+}
+
+/**
+ * @brief Update 3D vector Butterworth filters with new rate input data.
+ *
+ * @param[in,out] filter Struct containing Butterworth filters for x, y, z components.
+ * @param[in] input Pointer to FloatRates struct containing new input rate data.
+ */
+static inline struct FloatRates update_butterworth_4_low_pass_rates(struct Butterworth4LowPassVect3 *filter,
+ const struct FloatRates *input)
+{
+ update_butterworth_4_low_pass(&filter->x, input->p);
+ update_butterworth_4_low_pass(&filter->y, input->q);
+ update_butterworth_4_low_pass(&filter->z, input->r);
+ return get_butterworth_4_low_pass_rates(filter);
+}
+
+/**
+ * @brief Reset 3D vector Butterworth filters to a specific value.
+ *
+ * @param[out] filter Struct containing Butterworth filters for x, y, z components.
+ * @param[in] value Pointer to FloatVect3 struct containing the reset value.
+ */
+static inline void reset_butterworth_4_low_pass_vect3(struct Butterworth4LowPassVect3 *filter,
+ const struct FloatVect3 *value)
+{
+ reset_butterworth_4_low_pass(&filter->x, value->x);
+ reset_butterworth_4_low_pass(&filter->y, value->y);
+ reset_butterworth_4_low_pass(&filter->z, value->z);
+}
+
+/**
+ * @brief Reset 3D vector Butterworth filters to specific rate values.
+ *
+ * @param[out] filter Struct containing Butterworth filters for x, y, z components.
+ * @param[in] value Pointer to FloatRates struct containing the reset values.
+ */
+static inline void reset_butterworth_4_low_pass_rates(struct Butterworth4LowPassVect3 *filter,
+ const struct FloatRates *value)
+{
+ reset_butterworth_4_low_pass(&filter->x, value->p);
+ reset_butterworth_4_low_pass(&filter->y, value->q);
+ reset_butterworth_4_low_pass(&filter->z, value->r);
+}
+
+/**
+ * @brief Initialize an array of Butterworth low-pass filters to zero.
+ *
+ * @param[in] n Number of filters to initialize.
+ * @param[out] filter_array Array of Butterworth4LowPass filters to initialize.
+ * @param[in] cut_off Time constant for the filters (s).
+ * @param[in] dt Sampling time interval (seconds).
+ */
+static inline void init_butterworth_4_low_pass_array(const uint8_t n, Butterworth4LowPass filter_array[restrict n],
+ const float cut_off[restrict n], const float dt)
+{
+ for (uint8_t i = 0; i < n; i++) {
+ init_butterworth_4_low_pass(&filter_array[i], 1.0f / cut_off[i], dt, 0.0f);
+ }
+}
+
+/**
+ * @brief Update an array of Butterworth low-pass filters with new input data.
+ *
+ * @param[in] n Number of filters in the array.
+ * @param[in,out] filter_array Array of Butterworth4LowPass filters to update.
+ * @param[in] input_array Array containing new input data for each filter.
+ */
+static inline void update_butterworth_4_low_pass_array(const uint8_t n, Butterworth4LowPass filter_array[restrict n],
+ const float input_array[restrict n])
+{
+ for (uint8_t i = 0; i < n; i++) {
+ update_butterworth_4_low_pass(&filter_array[i], input_array[i]);
+ }
+}
+
+/**
+ * @brief Reset an array of Butterworth low-pass filters to specific values.
+ *
+ * @param[in] n Number of filters in the array.
+ * @param[out] filter_array Array of Butterworth4LowPass filters to reset.
+ * @param[in] value_array Array containing reset values for each filter.
+ */
+static inline void reset_butterworth_4_low_pass_array(const uint8_t n, Butterworth4LowPass filter_array[restrict n],
+ const float value_array[restrict n])
+{
+ for (uint8_t i = 0; i < n; i++) {
+ reset_butterworth_4_low_pass(&filter_array[i], value_array[i]);
+ }
+}
+
+/**
+ * @brief Retrieve the filtered outputs from an array of Butterworth low-pass filters.
+ *
+ * @param[in] n Number of filters in the array.
+ * @param[in] filter_array Array of Butterworth4LowPass filters.
+ * @param[out] output_array Array to store the filtered output values.
+ */
+static inline void get_butterworth_4_low_pass_array(const uint8_t n, const Butterworth4LowPass filter_array[restrict n],
+ float output_array[restrict n])
+{
+ for (uint8_t i = 0; i < n; i++) {
+ output_array[i] = get_butterworth_4_low_pass(&filter_array[i]);
+ }
+}
+
+#endif // LOW_PASS_FILTER_TYPES_H
diff --git a/sw/airborne/filters/low_pass_zoh_filter.h b/sw/airborne/filters/low_pass_zoh_filter.h
new file mode 100644
index 0000000000..5861e0b5af
--- /dev/null
+++ b/sw/airborne/filters/low_pass_zoh_filter.h
@@ -0,0 +1,116 @@
+/*
+ * Copyright (C) 2025 Justin Dubois
+ *
+ * This file is part of paparazzi
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ */
+/** @file low_pass_zoh_filter.h
+ * @brief A first order low pass filter using Zero Order Hold (ZOH) discretization.
+ */
+
+#ifndef LOW_PASS_ZOH_FILTER_H
+#define LOW_PASS_ZOH_FILTER_H
+
+#include "std.h"
+#include "math/pprz_algebra_int.h"
+
+
+/**
+ * @brief Zero Order Hold (ZOH) discrete first order low pass filter structure.
+ *
+ * This structure represents a first order low pass filter implemented using
+ * the Zero Order Hold (ZOH) method for discretization. It maintains the necessary
+ * state for filtering operations.
+ */
+struct FirstOrderZOHLowPass {
+ float discrete_time_constant;
+ float last_in;
+ float last_out;
+};
+
+/**
+ * @brief Init first order ZOH low pass filter.
+ *
+ * The ZOH discretization method is used to convert a continuous-time first order
+ * low pass filter into its discrete-time equivalent which matches the continuous-time
+ * behavior at the sampling instants exactly when holding the input constant between samples.
+ *
+ * @param[out] filter first order ZOH low pass filter structure
+ * @param[in] tau time constant of the first order low pass filter [s]
+ * @param[in] sample_time sampling period of the signal [s]
+ * @param[in] value initial value of the filter
+ */
+static inline void init_first_order_zoh_low_pass(struct FirstOrderZOHLowPass *filter, const float tau,
+ const float sample_time,
+ float value)
+{
+ filter->discrete_time_constant = exp(-sample_time / tau);
+ filter->last_in = value;
+ filter->last_out = value;
+}
+
+/**
+ * @brief Update first order ZOH low pass filter state with a new value.
+ *
+ * @param[in,out] filter first order ZOH low pass filter structure
+ * @param[in] value new input value of the filter
+ * @return new filtered value
+ */
+static inline float update_first_order_zoh_low_pass(struct FirstOrderZOHLowPass *filter, const float value)
+{
+ float out = (1.0f - filter->discrete_time_constant) * value + filter->discrete_time_constant * filter->last_out;
+ filter->last_in = value;
+ filter->last_out = out;
+ return out;
+}
+
+/**
+ * @brief Reset the first order ZOH low-pass filter to a specific value.
+ *
+ * @param[in,out] filter first order ZOH low pass filter structure
+ * @param[in] value Value to reset the filter to
+ * @return The reset value
+ */
+static inline float reset_first_order_zoh_low_pass(struct FirstOrderZOHLowPass *filter, const float value)
+{
+ filter->last_in = value;
+ filter->last_out = value;
+ return value;
+}
+
+/**
+ * @brief Get current value of the first order ZOH low pass filter.
+ *
+ * @param[in] filter first order ZOH low pass filter structure
+ * @return current value of the filter
+ */
+static inline float get_first_order_zoh_low_pass(const struct FirstOrderZOHLowPass *filter)
+{
+ return filter->last_out;
+}
+
+/** @brief Update time constant (tau parameter) for first order ZOH low pass filter
+ * @param[in,out] filter first order ZOH low pass filter structure
+ * @param[in] tau time constant of the first order low pass filter [s]
+ * @param[in] sample_time sampling period of the signal [s]
+ */
+static inline void update_first_order_zoh_low_pass_tau(struct FirstOrderZOHLowPass *filter, const float tau,
+ const float sample_time)
+{
+ filter->discrete_time_constant = exp(-sample_time / tau);
+}
+
+#endif // LOW_PASS_ZOH_FILTER_H
\ No newline at end of file
diff --git a/sw/airborne/filters/low_pass_zoh_filter_types.h b/sw/airborne/filters/low_pass_zoh_filter_types.h
new file mode 100644
index 0000000000..fecb00a690
--- /dev/null
+++ b/sw/airborne/filters/low_pass_zoh_filter_types.h
@@ -0,0 +1,97 @@
+/*
+ * Copyright (C) 2025 Justin Dubois
+ *
+ * This file is part of paparazzi
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ */
+/** @file low_pass_zoh_filter.h
+ * @brief A first order low pass filter using Zero Order Hold (ZOH) discretization.
+ */
+/** @file filters/low_pass_zoh_filter.h
+ * @brief Definitions and inline functions for 1st order low-pass ZOH filter vector types
+ *
+ * Provides structures and functions to initialize, update, reset, and get outputs from
+ * 1st order low-pass ZOH filter vectors.
+ *
+ * @author Justin Dubois
+ */
+
+#ifndef LOW_PASS_ZOH_FILTER_TYPES_H
+#define LOW_PASS_ZOH_FILTER_TYPES_H
+
+#include "std.h"
+#include "filters/low_pass_zoh_filter.h"
+#include "math/pprz_algebra_float.h"
+
+/**
+ * @brief Initialize a set of first order ZOH low-pass filters to zero for 3D vector data.
+ * @param[out] filter_array Array of FirstOrderZOHLowPass filters to initialize.
+ * @param[in] omega Cut-off frequencies for the filters (rad/s).
+ * @param[in] sample_time Sampling time interval (seconds).
+ */
+static inline void init_first_order_zoh_low_pass_array(const uint8_t n,
+ struct FirstOrderZOHLowPass filter_array[restrict n], const float omega[restrict n], const float sample_time)
+{
+ for (uint8_t i = 0; i < n; i++) {
+ float tau = 1.0f / omega[i];
+ init_first_order_zoh_low_pass(&filter_array[i], tau, sample_time, 0.0f);
+ }
+}
+
+/**
+ * @brief Update an array of first order ZOH low-pass filters.
+ * @param[in] n Number of filters in the array.
+ * @param[in,out] filter_array Array of FirstOrderZOHLowPass filters to update.
+ * @param[in] input_array Array of input values for the filters.
+ */
+static inline void update_first_order_zoh_low_pass_array(const uint8_t n,
+ struct FirstOrderZOHLowPass filter_array[restrict n], const float input_array[restrict n])
+{
+ for (uint8_t i = 0; i < n; i++) {
+ update_first_order_zoh_low_pass(&filter_array[i], input_array[i]);
+ }
+}
+
+/**
+ * @brief Reset an array of first order ZOH low-pass filters to specific values.
+ * @param[in] n Number of filters in the array.
+ * @param[out] filter_array Array of FirstOrderZOHLowPass filters to reset.
+ * @param[in] value_array Array of values to reset the filters to.
+ */
+static inline void reset_first_order_zoh_low_pass_array(const uint8_t n,
+ struct FirstOrderZOHLowPass filter_array[restrict n], const float value_array[restrict n])
+{
+ for (uint8_t i = 0; i < n; i++) {
+ filter_array[i].last_in = value_array[i];
+ filter_array[i].last_out = value_array[i];
+ }
+}
+
+/**
+ * @brief Retrieve the filtered outputs from an array of first order ZOH low-pass filters.
+ * @param[in] n Number of filters in the array.
+ * @param[in] filter_array Array of FirstOrderZOHLowPass filters.
+ * @param[out] output_array Array to store the filtered output values.
+ */
+static inline void get_first_order_zoh_low_pass_array(const uint8_t n,
+ const struct FirstOrderZOHLowPass filter_array[restrict n], float output_array[restrict n])
+{
+ for (uint8_t i = 0; i < n; i++) {
+ output_array[i] = filter_array[i].last_out;
+ }
+}
+
+#endif // LOW_PASS_ZOH_FILTER_TYPES_H
\ No newline at end of file
diff --git a/sw/airborne/filters/transport_delay.h b/sw/airborne/filters/transport_delay.h
new file mode 100644
index 0000000000..73a638a610
--- /dev/null
+++ b/sw/airborne/filters/transport_delay.h
@@ -0,0 +1,104 @@
+/*
+ * Copyright (C) 2025 Justin Dubois
+ *
+ * This file is part of paparazzi
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ */
+/** @file transport_delay.h
+ * @brief Transport delay filter implementation.
+ */
+
+#ifndef TRANSPORT_DELAY_H
+#define TRANSPORT_DELAY_H
+
+#include "paparazzi.h"
+
+#define TRANSPORT_DELAY_BUFFER_SIZE 20
+
+struct TransportDelay {
+ uint8_t delay_samples; // Number of samples to delay
+ uint8_t write_index; // Current write index
+ float buffer[TRANSPORT_DELAY_BUFFER_SIZE];
+};
+
+
+/**
+ * Initialize a transport delay buffer.
+ *
+ * @param[out] td Pointer to the transport_delay_t structure to initialize.
+ * @param[in] delay_samples Number of samples to delay. If this value exceeds TRANSPORT_DELAY_BUFFER_SIZE,
+ * it will be clamped to TRANSPORT_DELAY_BUFFER_SIZE.
+ * @param[in] initial_value Initial value to fill the buffer with.
+ *
+ * @note: If delay_samples > TRANSPORT_DELAY_BUFFER_SIZE, it will be clamped to TRANSPORT_DELAY_BUFFER_SIZE
+ * and the requested delay will not be fully honored.
+ */
+static inline void init_transport_delay(struct TransportDelay *td, uint8_t delay_samples, const float initial_value)
+{
+ if (delay_samples > TRANSPORT_DELAY_BUFFER_SIZE) {
+ delay_samples = TRANSPORT_DELAY_BUFFER_SIZE;
+ }
+ td->delay_samples = delay_samples;
+ td->write_index = 0;
+ for (uint8_t i = 0; i < TRANSPORT_DELAY_BUFFER_SIZE; i++) {
+ td->buffer[i] = initial_value;
+ }
+}
+
+/**
+ * Propagate a new input value through the transport delay buffer.
+ *
+ * @param[in,out] td Pointer to the transport_delay_t structure.
+ * @param[in] input New input value to add to the buffer.
+ * @return Delayed output value from the buffer.
+ */
+static inline float update_transport_delay(struct TransportDelay *td, const float input)
+{
+ td->buffer[td->write_index] = input;
+ uint8_t read_index = (td->write_index + TRANSPORT_DELAY_BUFFER_SIZE - td->delay_samples) % TRANSPORT_DELAY_BUFFER_SIZE;
+ float output = td->buffer[read_index];
+ td->write_index = (td->write_index + 1) % TRANSPORT_DELAY_BUFFER_SIZE;
+ return output;
+}
+
+/**
+ * Reset the transport delay buffer to a specific initial value.
+ *
+ * @param[in,out] td Pointer to the transport_delay_t structure.
+ * @param[in] initial_value Value to reset the buffer elements to.
+ */
+static inline void reset_transport_delay(struct TransportDelay *td, const float initial_value)
+{
+ td->write_index = 0;
+ for (uint8_t i = 0; i < TRANSPORT_DELAY_BUFFER_SIZE; i++) {
+ td->buffer[i] = initial_value;
+ }
+}
+
+/**
+ * Get the current output value from the transport delay buffer without updating it.
+ *
+ * @param td Pointer to the transport_delay_t structure.
+ * @return Current delayed output value from the buffer.
+ */
+static inline float get_transport_delay(const struct TransportDelay *td)
+{
+ uint8_t read_index = (td->write_index + TRANSPORT_DELAY_BUFFER_SIZE - td->delay_samples - 1) %
+ TRANSPORT_DELAY_BUFFER_SIZE;
+ return td->buffer[read_index];
+}
+
+#endif // TRANSPORT_DELAY_H
\ No newline at end of file
diff --git a/sw/airborne/filters/transport_delay_types.h b/sw/airborne/filters/transport_delay_types.h
new file mode 100644
index 0000000000..686732b9e4
--- /dev/null
+++ b/sw/airborne/filters/transport_delay_types.h
@@ -0,0 +1,88 @@
+/*
+ * Copyright (C) 2025 Justin Dubois
+ *
+ * This file is part of paparazzi
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ */
+/** @file transport_delay_types.h
+ * @brief Transport delay filter type definitions and array operations.
+ */
+
+#ifndef TRANSPORT_DELAY_TYPES_H
+#define TRANSPORT_DELAY_TYPES_H
+
+#include "std.h"
+#include "paparazzi.h"
+#include "filters/transport_delay.h"
+
+/**
+ * @brief Initialize an array of TransportDelay structures.
+ * @param[in] n Number of TransportDelay structures in the array.
+ * @param[in,out] td_array Array of TransportDelay structures to initialize.
+ * @param[in] delay_samples Array of delay samples for each TransportDelay structure.
+ * @param[in] initial_value Array of initial values to fill the buffers.
+ */
+static inline void init_transport_delay_array(uint8_t n, struct TransportDelay td_array[restrict n],
+ const uint8_t delay_samples[restrict n], float initial_value[restrict n])
+{
+ for (uint8_t i = 0; i < n; i++) {
+ init_transport_delay(&td_array[i], delay_samples[i], initial_value[i]);
+ }
+}
+
+/**
+ * @brief Update an array of TransportDelay structures with input values.
+ * @param[in] n Number of TransportDelay structures in the array.
+ * @param[in,out] td_array Array of TransportDelay structures to update.
+ * @param[in] input_array Array of input values for each TransportDelay structure.
+ */
+static inline void update_transport_delay_array(uint8_t n, struct TransportDelay td_array[restrict n],
+ const float input_array[restrict n])
+{
+ for (uint8_t i = 0; i < n; i++) {
+ update_transport_delay(&td_array[i], input_array[i]);
+ }
+}
+
+/**
+ * @brief Reset an array of TransportDelay structures to specific initial values.
+ * @param[in] n Number of TransportDelay structures in the array.
+ * @param[in,out] td_array Array of TransportDelay structures to reset.
+ * @param[in] initial_value Array of initial values to reset the buffers to.
+ */
+static inline void reset_transport_delay_array(const uint8_t n, struct TransportDelay td_array[restrict n],
+ const float initial_value[restrict n])
+{
+ for (uint8_t i = 0; i < n; i++) {
+ reset_transport_delay(&td_array[i], initial_value[i]);
+ }
+}
+
+/**
+ * @brief Get output values from an array of TransportDelay structures.
+ * @param[in] n Number of TransportDelay structures in the array.
+ * @param[in] td_array Array of TransportDelay structures to get outputs from.
+ * @param[out] output_array Array to store the output values for each TransportDelay structure.
+ */
+static inline void get_transport_delay_array(const uint8_t n, const struct TransportDelay td_array[restrict n],
+ float output_array[restrict n])
+{
+ for (uint8_t i = 0; i < n; i++) {
+ output_array[i] = get_transport_delay(&td_array[i]);
+ }
+}
+
+#endif // TRANSPORT_DELAY_TYPES_H
\ No newline at end of file
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/obm_cyclone.c b/sw/airborne/firmwares/rotorcraft/stabilization/obm_cyclone.c
new file mode 100644
index 0000000000..e3714ad49b
--- /dev/null
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/obm_cyclone.c
@@ -0,0 +1,378 @@
+#include "firmwares/rotorcraft/stabilization/stabilization_andi.h"
+#include
+
+#if ANDI_NUM_ACT != 4
+ #error Cyclone expects 4 actuators
+#endif
+
+#if ANDI_OUTPUTS != 4
+ #error The cyclone model provides 4 outputs
+#endif
+
+union CycloneCoefficients {
+ struct {
+ // X-axis force coefficients (f_ff_x)
+ float fx_motor_squared; // Motor thrust squared effect
+ float fx_speed_forward; // Forward speed effect
+
+ // Y-axis force coefficients (f_ff_y)
+ float fy_speed_lateral; // Lateral speed effect
+
+ // Z-axis force coefficients (f_ff_z)
+ float fz_motor_squared; // Motor thrust squared effect
+ float fz_speed_forward; // Forward speed effect
+ float fz_speed_vertical; // Vertical speed effect
+ float fz_elevon_speed; // Elevon-speed coupling
+ float fz_elevon_motor; // Elevon-motor coupling
+
+ // X-axis moment coefficients (m_ff_x)
+ float mx_elevon_motor_diff; // (ele_l * motor_l^2 - ele_r * motor_r^2)
+ float mx_elevon_speed_diff; // (ele_l - ele_r) * speed * v_ff(1)
+ float mx_angular_drag; // w_ff(1)^2
+ float mx_angular_coupling; // w_ff(2) * w_ff(3)
+
+ // Y-axis moment coefficients (m_ff_y)
+ float my_speed_forward; // speed * v_ff(1)
+ float my_speed_vertical; // speed * v_ff(3)
+ float my_motor_sum; // motor_l^2 + motor_r^2
+ float my_elevon_motor_sum; // ele_l * motor_l^2 + ele_r * motor_r^2
+ float my_elevon_speed_sum; // (ele_l + ele_r) * speed * v_ff(1)
+ float my_angular_sum; // w_ff(1) + w_ff(3)
+
+ // Z-axis moment coefficients (m_ff_z)
+ float mz_speed_lateral; // speed * v_ff(2)
+ float mz_motor_diff; // motor_l^2 - motor_r^2
+ float mz_speed_roll; // speed * w_ff(1)
+ float mz_angular_coupling; // w_ff(1) * w_ff(2)
+
+ // Fixme: Move to x-axis moment coefficients
+ };
+ float data[22];
+};
+
+// Model coefficinets
+union CycloneCoefficients obm_coefficients = {
+ .fx_motor_squared = 0.00000735f,
+ .fx_speed_forward = -0.03f,
+
+ .fy_speed_lateral = -0.008f,
+
+ .fz_motor_squared = 0.0f,
+ .fz_speed_forward = 0.0f,
+ .fz_speed_vertical = -0.144f,
+ .fz_elevon_speed = 0.0f,
+ .fz_elevon_motor = 0.0f,
+
+ .mx_elevon_motor_diff = 1.9e-05f,
+ .mx_elevon_speed_diff = 0.344f,
+ .mx_angular_drag = -0.4940217f,
+ .mx_angular_coupling = -2.18f,
+
+ .my_speed_forward = 0.0f,
+ .my_speed_vertical = -0.0888f,
+ .my_motor_sum = 0.0f,
+ .my_elevon_motor_sum = -0.0000424f,
+ .my_elevon_speed_sum = 0.2525f,
+ .my_angular_sum = 1.262f,
+
+ .mz_speed_lateral = -0.00371f,
+ .mz_motor_diff = 0.000039f,
+ .mz_speed_roll = -0.0129f,
+ .mz_angular_coupling = -0.4827f,
+};
+
+union CeMatrix {
+ struct {
+ float ce_11;
+ float ce_12;
+ float ce_13;
+ float ce_14;
+ float ce_21;
+ float ce_22;
+ float ce_23;
+ float ce_24;
+ float ce_31;
+ float ce_32;
+ float ce_33;
+ float ce_34;
+ float ce_41;
+ float ce_42;
+ float ce_43;
+ float ce_44;
+
+ };
+ float data[16];
+};
+
+// Model coefficinets
+union CeMatrix ce_mat_tmp = {
+ .ce_11 = 0.0f, .ce_12 = 0.0f, .ce_13 = 3.9e-5f, .ce_14 = -3.9e-5f, // Roll
+ .ce_21 = -29.917439f, .ce_22 = -29.917439f, .ce_23 = 0.0f, .ce_24 = 0.0f, // Pitch
+ .ce_31 = -19.968481f, .ce_32 = 19.968481f, .ce_33 = 0.0f, .ce_34 = 0.0f, // Yaw
+ .ce_41 = 0.0f, .ce_42 = 0.0f, .ce_43 = 7e-6f, .ce_44 = 7e-6f, // Thrust
+};
+
+
+/* Function Definitions */
+static void cyclone_obm_forces(const float body_vel[3], const float u[4],
+ const float coeff[22], float F_obm_forces[3])
+{
+ float t2;
+ float t7;
+ t2 = u[2] + u[3];
+ t7 = sqrtf((body_vel[0] * body_vel[0] + body_vel[1] * body_vel[1]) + body_vel[2] * body_vel[2]);
+ F_obm_forces[0] = coeff[3] * t2
+ + coeff[7] * (u[0] * u[2] + u[1] * u[3])
+ + coeff[5] * t7 * body_vel[0]
+ - coeff[4] * t7 * body_vel[2]
+ - coeff[6] * t7 * body_vel[2] * (u[0] + u[1]);
+ F_obm_forces[1] = coeff[2] * t7 * body_vel[1];
+ F_obm_forces[2] = -coeff[0] * t2 + coeff[1] * t7 * body_vel[2];
+}
+
+static void cyclone_obm_moments(const float rates[3],
+ const float body_vel[3], const float u[4],
+ const float coeff[22], float F_obm_moments[3])
+{
+ float t2;
+ float t3;
+ float t8;
+ t2 = u[0] * u[2];
+ t3 = u[1] * u[3];
+ t8 = sqrtf((body_vel[0] * body_vel[0] + body_vel[1] * body_vel[1]) + body_vel[2] * body_vel[2]);
+ F_obm_moments[0] = coeff[19] * (u[2] - u[3])
+ + coeff[18] * t8 * body_vel[1]
+ - coeff[20] * t8 * rates[2]
+ - rates[1] * coeff[21] * rates[2];
+ F_obm_moments[1] = coeff[15] * (t2 + t3)
+ + coeff[14] * (u[2] + u[3])
+ + coeff[13] * t8 * body_vel[0]
+ - coeff[12] * t8 * body_vel[2]
+ - rates[0] * coeff[17] * rates[2]
+ - coeff[16] * t8 * body_vel[2] * (u[0] + u[1]);
+ F_obm_moments[2] = -coeff[8] * (t2 - t3)
+ + coeff[10] * fabsf(rates[2]) * rates[2]
+ - rates[0] * coeff[11] * rates[1]
+ + coeff[9] * t8 * body_vel[2] * (u[0] - u[1]);
+}
+
+
+static void cyclone_f_stb_u(const float body_vel[3],
+ const float u[4], const float coeff[22], float F_stb_u[16])
+{
+ float fv[16];
+ float t6;
+ float t7;
+ int i;
+ int i1;
+ int i2;
+ t6 = sqrtf((body_vel[0] * body_vel[0] + body_vel[1] * body_vel[1]) + body_vel[2] * body_vel[2]);
+ t7 = coeff[9] * t6 * body_vel[2];
+ t6 = coeff[16] * t6 * body_vel[2];
+ fv[0] = 0.0F;
+ fv[1] = -t6 + u[2] * coeff[15];
+ fv[2] = t7 - u[2] * coeff[8];
+ fv[3] = 0.0F;
+ fv[4] = 0.0F;
+ fv[5] = -t6 + u[3] * coeff[15];
+ fv[6] = -t7 + u[3] * coeff[8];
+ fv[7] = 0.0F;
+ fv[8] = coeff[19];
+ fv[9] = coeff[14] + u[0] * coeff[15];
+ fv[10] = -coeff[8] * u[0];
+ fv[11] = coeff[0];
+ fv[12] = -coeff[19];
+ fv[13] = coeff[14] + u[1] * coeff[15];
+ fv[14] = u[1] * coeff[8];
+ fv[15] = coeff[0];
+ i = 0;
+ i1 = 0;
+ for (i2 = 0; i2 < 16; i2++) {
+ F_stb_u[i1 + (i << 2)] = fv[i2];
+ i++;
+ if (i > 3) {
+ i = 0;
+ i1++;
+ }
+ }
+}
+
+/* Function Definitions */
+void cyclone_f_stb_x(const float rates[3], const float ang_accel[3],
+ const float vel_body[3], const float accel_body[3], const float u[4],
+ const float coeff[22], float F_stb_x[4])
+{
+ float F_stb_x_tmp;
+ float b_F_stb_x_tmp;
+ float c_F_stb_x_tmp;
+ float d_F_stb_x_tmp;
+ float et1_tmp;
+ float t10;
+ float t12;
+ float t15;
+ float t8;
+ float t9;
+ t8 = vel_body[0] * vel_body[0];
+ t9 = vel_body[1] * vel_body[1];
+ t10 = vel_body[2] * vel_body[2];
+ t12 = u[0] - u[1];
+ t15 = sqrtf((t8 + t9) + t10);
+ et1_tmp = fmaxf(1.0E-8F, t15);
+ F_stb_x[0] = -(
+ -accel_body[1] * coeff[18] * t9
+ - accel_body[0] * coeff[18] * vel_body[0] * vel_body[1]
+ - accel_body[2] * coeff[18] * vel_body[1] * vel_body[2]
+ + accel_body[0] * coeff[20] * vel_body[0] * rates[2]
+ + accel_body[1] * coeff[20] * vel_body[1] * rates[2]
+ + accel_body[2] * coeff[20] * vel_body[2] * rates[2]
+ - accel_body[1] * coeff[18] * t15 * et1_tmp
+ + coeff[20] * t15 * ang_accel[2] * et1_tmp
+ + rates[1] * coeff[21] * ang_accel[2] * et1_tmp
+ + rates[2] * coeff[21] * ang_accel[1] * et1_tmp
+ ) / et1_tmp;
+
+ t9 = accel_body[2] * coeff[12];
+ F_stb_x_tmp = accel_body[2] * coeff[16];
+ b_F_stb_x_tmp = accel_body[0] * coeff[16];
+ c_F_stb_x_tmp = accel_body[1] * coeff[16];
+ d_F_stb_x_tmp = F_stb_x_tmp * u[0];
+ F_stb_x_tmp *= u[1];
+ F_stb_x[1] = -(
+ -accel_body[0] * coeff[13] * t8 + t9 * t10
+ + accel_body[0] * coeff[12] * vel_body[0] * vel_body[2]
+ - accel_body[1] * coeff[13] * vel_body[0] * vel_body[1]
+ + accel_body[1] * coeff[12] * vel_body[1] * vel_body[2]
+ - accel_body[1] * coeff[12] * vel_body[1] * vel_body[2]
+ - accel_body[2] * coeff[13] * vel_body[0] * vel_body[2]
+ - accel_body[0] * coeff[13] * t15 * et1_tmp
+ + t9 * t15 * et1_tmp
+ + rates[0] * coeff[17] * ang_accel[2] * et1_tmp
+ + rates[2] * coeff[17] * ang_accel[0] * et1_tmp
+ + d_F_stb_x_tmp * t10
+ + F_stb_x_tmp * t10
+ + b_F_stb_x_tmp * u[0] * vel_body[0] * vel_body[2]
+ + c_F_stb_x_tmp * u[0] * vel_body[1] * vel_body[2]
+ + b_F_stb_x_tmp * u[1] * vel_body[0] * vel_body[2]
+ + c_F_stb_x_tmp * u[1] * vel_body[1] * vel_body[2]
+ + d_F_stb_x_tmp * t15 * et1_tmp
+ + F_stb_x_tmp * t15 * et1_tmp
+ ) / et1_tmp;
+ F_stb_x[2] = accel_body[2] * (coeff[9] * t12 * t15 + coeff[9] * t10 * t12 / et1_tmp)
+ - rates[0] * coeff[11] * ang_accel[1]
+ - rates[1] * coeff[11] * ang_accel[0]
+ + 2.0F * coeff[10] * fabsf(rates[2]) * ang_accel[2]
+ + accel_body[0] * coeff[9] * t12 * vel_body[0] * vel_body[2] / et1_tmp
+ + accel_body[1] * coeff[9] * t12 * vel_body[1] * vel_body[2] / et1_tmp;
+ F_stb_x[3] = 0.0F;
+}
+
+
+
+/* End of code generation (cyclone_f_stb_x.c) */
+
+struct FloatVect3 evaluate_obm_forces(const struct FloatRates *rates, const struct FloatVect3 *vel_body,
+ const float actuator_state[ANDI_NUM_ACT])
+{
+ float vel_body_array[3];
+ (void)rates;
+
+ vel_body_array[0] = vel_body->x;
+ vel_body_array[1] = vel_body->y;
+ vel_body_array[2] = vel_body->z;
+
+ float forces_array[3];
+ cyclone_obm_forces(vel_body_array, actuator_state, obm_coefficients.data, forces_array);
+
+ struct FloatVect3 forces;
+ forces.x = forces_array[0];
+ forces.y = forces_array[1];
+ forces.z = forces_array[2];
+
+ return forces;
+}
+
+struct FloatVect3 evaluate_obm_moments(const struct FloatRates *rates, const struct FloatVect3 *vel_body,
+ const float actuator_state[ANDI_NUM_ACT])
+{
+ float vel_body_array[3];
+ float rates_array[3];
+
+ vel_body_array[0] = vel_body->x;
+ vel_body_array[1] = vel_body->y;
+ vel_body_array[2] = vel_body->z;
+
+ rates_array[0] = rates->p;
+ rates_array[1] = rates->q;
+ rates_array[2] = rates->r;
+
+ float moments_array[3];
+ cyclone_obm_moments(rates_array, vel_body_array, actuator_state, obm_coefficients.data, moments_array);
+ struct FloatVect3 moments;
+ moments.x = moments_array[0];
+ moments.y = moments_array[1];
+ moments.z = moments_array[2];
+
+ return moments;
+}
+
+
+void evaluate_obm_f_stb_u(float fu_mat[ANDI_NUM_ACT * ANDI_OUTPUTS], const struct FloatRates *rates,
+ const struct FloatVect3 *vel_body, const float actuator_state[ANDI_NUM_ACT])
+{
+ float vel_body_array[3];
+ (void)rates;
+
+ vel_body_array[0] = vel_body->x;
+ vel_body_array[1] = vel_body->y;
+ vel_body_array[2] = vel_body->z;
+
+ // Bound min motor speed in actuator_state to prevent really low control effectiveness which may result in instabilities.
+ // This should make "free fall" more stable at the cost of some model inaccuracy at very low thrust.
+ // FIXME: Rethink this solution.
+ // FIXME: Apply this bounding directly on the elevon effectiveness terms in the final matrix instead of modifying actuator_state.
+ // FIXME: Would it be possible to dynamically identify or saturate the control effectiveness?
+ float actuator_state_bounded[ANDI_NUM_ACT];
+ actuator_state_bounded[0] = actuator_state[0];
+ actuator_state_bounded[1] = actuator_state[1];
+ actuator_state_bounded[2] = fmaxf(actuator_state[2], 360000.0f);
+ actuator_state_bounded[3] = fmaxf(actuator_state[3], 360000.0f);
+
+ cyclone_f_stb_u(vel_body_array, actuator_state_bounded, obm_coefficients.data, fu_mat);
+
+ fu_mat = ce_mat_tmp.data; // FIXME: Temporary hack to use ce_mat instead of the real computed matrix
+}
+
+void evaluate_obm_f_stb_x(float nu_obm[ANDI_OUTPUTS], const struct FloatRates *rates,
+ const struct FloatVect3 *vel_body, const struct FloatVect3 *ang_accel, const struct FloatVect3 *accel_body,
+ const float actuator_state[ANDI_NUM_ACT])
+{
+ float rates_array[3];
+ float ang_accel_array[3];
+ float vel_body_array[3];
+ float accel_body_array[3];
+
+ rates_array[0] = rates->p;
+ rates_array[1] = rates->q;
+ rates_array[2] = rates->r;
+
+ ang_accel_array[0] = ang_accel->x;
+ ang_accel_array[1] = ang_accel->y;
+ ang_accel_array[2] = ang_accel->z;
+
+ vel_body_array[0] = vel_body->x;
+ vel_body_array[1] = vel_body->y;
+ vel_body_array[2] = vel_body->z;
+
+ accel_body_array[0] = accel_body->x;
+ accel_body_array[1] = accel_body->y;
+ accel_body_array[2] = accel_body->z;
+
+
+ cyclone_f_stb_x(rates_array, ang_accel_array, vel_body_array, accel_body_array, actuator_state, obm_coefficients.data,
+ nu_obm);
+}
+
+float evaluate_obm_thrust_z(const float actuator_state[ANDI_NUM_ACT])
+{
+ return obm_coefficients.fx_motor_squared * (actuator_state[2] + actuator_state[3]);
+}
\ No newline at end of file
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_andi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_andi.c
new file mode 100644
index 0000000000..970fa1c2b4
--- /dev/null
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_andi.c
@@ -0,0 +1,1444 @@
+/*
+ *
+ * Copyright (C) 2025 Justin Dubois
+ *
+ * This file is part of paparazzi
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ *
+ */
+
+/**
+ * @file sw/airborne/firmwares/rotorcraft/stabilization/stabilization_andi.c
+ * @brief ANDI stabilization controller for tiltbody rotorcraft.
+ *
+ * Implements Adaptive Nonlinear Dynamic Inversion (ANDI) for attitude and thrust control
+ * with on-board model compensation.
+ *
+ * Assumptions:
+ * - Airframe is a tiltbody with 2 elevons (indices 0,1) and 2 motors (indices 2,3) in tractor configuration.
+ * - ESC neutral value equals zero thrust (usually 0).
+ * - Motors controlled in squared RPM to linearize thrust curve.
+ *
+ * FIXME: WLS allocation normalization depends on control effectiveness scaling (state-dependent).
+ * Consider normalizing costs by max achievable derivatives per actuator/output.
+ *
+ * @author Justin Dubois
+ */
+
+
+#include "firmwares/rotorcraft/stabilization/stabilization_andi.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
+#include "math/pprz_algebra_float.h"
+#include "state.h"
+#include "generated/airframe.h"
+#include "modules/radio_control/radio_control.h"
+#include "modules/actuators/actuators.h"
+#include "modules/core/abi.h"
+#include "math/wls/wls_alloc.h"
+#include "modules/nav/nav_rotorcraft_hybrid.h"
+#include "firmwares/rotorcraft/navigation.h"
+#include "modules/rotwing_drone/rotwing_state.h"
+#include "modules/core/commands.h"
+#include "autopilot.h"
+#include "filters/low_pass_filter.h"
+#include "filters/low_pass_filter_types.h"
+#include "filters/complementary_filter.h"
+#include "filters/complementary_filter_types.h"
+#include "filters/transport_delay.h"
+#include "filters/transport_delay_types.h"
+#include "filters/low_pass_zoh_filter.h"
+#include "filters/low_pass_zoh_filter_types.h"
+
+#include
+#if INS_EXT_POSE
+ #include "modules/ins/ins_ext_pose.h"
+#endif
+
+#ifdef STABILIZATION_ANDI_SCHEDULE_EFF
+ const bool SCHEDULE_EFF = STABILIZATION_ANDI_SCHEDULE_EFF;
+#else
+ const bool SCHEDULE_EFF = false;
+#endif
+
+#ifdef STABILIZATION_ANDI_USE_STATE_DYNAMICS
+ const bool USE_STATE_DYNAMICS = STABILIZATION_ANDI_USE_STATE_DYNAMICS;
+#else
+ const bool USE_STATE_DYNAMICS = false;
+#endif
+
+#ifdef STABILIZATION_ANDI_RELAX_OBM
+ const float ANDI_RELAX_OBM = STABILIZATION_ANDI_RELAX_OBM;
+#else
+ const float ANDI_RELAX_OBM = 1.0f;
+#endif
+
+#ifdef STABILIZATION_ANDI_ACT_IS_SERVO
+const bool ACTUATOR_IS_SERVO[ANDI_NUM_ACT] = STABILIZATION_ANDI_ACT_IS_SERVO;
+#else
+const bool ACTUATOR_IS_SERVO[ANDI_NUM_ACT] = {0};
+#endif
+
+#ifdef STABILIZATION_ANDI_ACT_DYNAMICS
+ const float ACTUATOR_DYNAMICS[ANDI_NUM_ACT] = STABILIZATION_ANDI_ACT_DYNAMICS; // rad/s
+#else
+ #error "You must specify the actuator dynamics"
+#endif
+
+#ifdef STABILIZATION_ANDI_ACT_DELAY
+const uint8_t ACTUATOR_DELAY[ANDI_NUM_ACT] = STABILIZATION_ANDI_ACT_DELAY;
+#else
+const uint8_t ACTUATOR_DELAY[ANDI_NUM_ACT] = {0};
+#endif
+
+#if defined(STABILIZATION_ANDI_ACT_MAX) && defined(STABILIZATION_ANDI_ACT_MIN)
+ const float ACTUATOR_MAX[ANDI_NUM_ACT] = STABILIZATION_ANDI_ACT_MAX;
+ const float ACTUATOR_MIN[ANDI_NUM_ACT] = STABILIZATION_ANDI_ACT_MIN;
+#else
+ #error "You must specify the actuator limits: STABILIZATION_ANDI_ACT_MAX and STABILIZATION_ANDI_ACT_MIN"
+#endif
+
+#if defined(STABILIZATION_ANDI_ACT_RATE_MAX) && defined(STABILIZATION_ANDI_ACT_RATE_MIN)
+ const float ACTUATOR_D_MAX[ANDI_NUM_ACT] = STABILIZATION_ANDI_ACT_RATE_MAX;
+ const float ACTUATOR_D_MIN[ANDI_NUM_ACT] = STABILIZATION_ANDI_ACT_RATE_MIN;
+#else
+ #error "You must specify the actuator limits: STABILIZATION_ANDI_ACT_RATE_MAX and STABILIZATION_ANDI_ACT_RATE_MIN"
+#endif
+
+#if defined STABILIZATION_ANDI_ACT_PREF
+const float ACTUATOR_PREF[ANDI_NUM_ACT] = STABILIZATION_ANDI_ACT_PREF;
+#else
+const float ACTUATOR_PREF[ANDI_NUM_ACT] = {0.0f};
+#endif
+
+#if defined STABILIZATION_ANDI_RC_RATE_MAX
+const float RC_RATE_MAX[ANDI_OUTPUTS] = STABILIZATION_ANDI_RC_RATE_MAX;
+#else
+const float RC_RATE_MAX[ANDI_OUTPUTS] = {[0 ... ANDI_OUTPUTS - 1] = 5.0f};
+#endif
+
+#ifdef STABILIZATION_ANDI_THRUST_MIN
+ const float THRUST_MIN = STABILIZATION_ANDI_THRUST_MIN;
+#else
+ const float THRUST_MIN = 0.0f;
+#endif
+
+#ifdef STABILIZATION_ANDI_THRUST_MAX
+ const float THRUST_MAX = STABILIZATION_ANDI_THRUST_MAX;
+#else
+ #error "You must specify maximum specific thrust: STABILIZATION_ANDI_THRUST_MAX"
+#endif
+
+#ifdef PERIODIC_FREQUENCY
+ const float SAMPLE_TIME = 1.0f / PERIODIC_FREQUENCY;
+#else
+ #error "Periodic frequency is not defined."
+#endif
+
+#if ANDI_NUM_ACT > WLS_N_U_MAX
+ #error Matrix-WLS_N_U_MAX too small or not defined: define WLS_N_U_MAX >= ANDI_NUM_ACT in airframe file
+#endif
+#if ANDI_OUTPUTS > WLS_N_V_MAX
+ #error Matrix-WLS_N_V_MAX too small or not defined: define WLS_N_V_MAX >= ANDI_OUTPUTS in airframe file
+#endif
+
+#ifdef STABILIZATION_ANDI_WLS_WV
+const float WLS_WV[ANDI_OUTPUTS] = STABILIZATION_ANDI_WLS_WV;
+#else
+const float WLS_WV[ANDI_OUTPUTS] = {[0 ... ANDI_OUTPUTS - 1] = 1.0f};
+#endif
+
+/**
+ * Normalized actuator cost for WLS allocation.
+ * Each value corresponds to the relative cost of using each actuator, normalized over
+ * the possible range of u_dot for that actuator.
+ * FIXME: This normalization is not ideal or constant, as it depends on the scaling of the control.
+ */
+#ifdef STABILIZATION_ANDI_WLS_WU
+float WLS_WU[ANDI_NUM_ACT] = STABILIZATION_ANDI_WLS_WU;
+#else
+float WLS_WU[ANDI_NUM_ACT] = {[0 ... ANDI_NUM_ACT - 1] = 1.0f};
+#endif
+
+#ifdef STABILIZATION_ANDI_CUTOFF_FREQ_OMEGA
+const struct FloatVect3 CUTOFF_FREQ_OMEGA = {
+ .x = STABILIZATION_ANDI_CUTOFF_FREQ_OMEGA,
+ .y = STABILIZATION_ANDI_CUTOFF_FREQ_OMEGA,
+ .z = STABILIZATION_ANDI_CUTOFF_FREQ_OMEGA
+}; // rad/s
+#else
+#error "You must specify the cutoff frequency for omega: STABILIZATION_ANDI_CUTOFF_FREQ_OMEGA"
+#endif
+
+#ifdef STABILIZATION_ANDI_CUTOFF_FREQ_OMEGA_DOT
+const struct FloatVect3 CUTOFF_FREQ_OMEGA_DOT = {
+ .x = STABILIZATION_ANDI_CUTOFF_FREQ_OMEGA_DOT,
+ .y = STABILIZATION_ANDI_CUTOFF_FREQ_OMEGA_DOT,
+ .z = STABILIZATION_ANDI_CUTOFF_FREQ_OMEGA_DOT
+}; // rad/s
+const float CUTOFF_FREQ_ACTUATOR[ANDI_NUM_ACT] = {[0 ... ANDI_NUM_ACT - 1] = STABILIZATION_ANDI_CUTOFF_FREQ_OMEGA_DOT}; // rad/s
+const float CUTOFF_FREQ_THRUST = STABILIZATION_ANDI_CUTOFF_FREQ_OMEGA_DOT; // rad/s
+#else
+#error "You must specify the cutoff frequency for omega_dot: STABILIZATION_ANDI_CUTOFF_FREQ_OMEGA_DOT"
+#endif
+
+#ifdef STABILIZATION_ANDI_CUTOFF_FREQ_ACCEL
+const struct FloatVect3 CUTOFF_FREQ_ACCEL = {
+ .x = STABILIZATION_ANDI_CUTOFF_FREQ_ACCEL,
+ .y = STABILIZATION_ANDI_CUTOFF_FREQ_ACCEL,
+ .z = STABILIZATION_ANDI_CUTOFF_FREQ_ACCEL
+};
+#else
+#error "You must specify the cutoff frequency for accel: STABILIZATION_ANDI_CUTOFF_FREQ_ACCEL"
+#endif
+
+#ifdef STABILIZATION_ANDI_CUTOFF_FREQ_VEL
+const struct FloatVect3 CUTOFF_FREQ_VEL = {
+ .x = STABILIZATION_ANDI_CUTOFF_FREQ_VEL,
+ .y = STABILIZATION_ANDI_CUTOFF_FREQ_VEL,
+ .z = STABILIZATION_ANDI_CUTOFF_FREQ_VEL
+}; // rad/s
+#else
+#error "You must specify the cutoff frequency for vel: STABILIZATION_ANDI_CUTOFF_FREQ_VEL"
+#endif
+
+
+// Function prototypes
+static void actuators_t4_in_callback(uint8_t sender_id, struct ActuatorsT4In *actuators_t4_in_ptr,
+ float *actuators_t4_extra_data_in_ptr);
+static void fetch_actuators_t4(float actuator_meas[ANDI_NUM_ACT], const struct ActuatorsT4In *actuators_t4_in_ptr);
+static struct GainsOrder3Vect3 compute_reference_gains_order_3_vect_3(const struct PolesOrder3Vect3 *poles);
+static struct GainsOrder2Vect3 compute_reference_gains_order_2_vect_3(const struct PolesOrder2Vect3 *poles);
+static struct GainsOrder3Vect3 compute_error_gains_order_3_vect_3(const struct PolesOrder3Vect3 *poles);
+static struct GainsOrder2Vect3 compute_error_gains_order_2_vect_3(const struct PolesOrder2Vect3 *poles);
+static void generate_reference_rate(float dt, const struct FloatRates *rate_des,
+ const struct GainsOrder2Vect3 *k_rate_rm, const struct AttQuat *bounds,
+ struct AttQuat *att_ref) __attribute__((unused));
+static void generate_reference_attitude(float dt, const struct FloatQuat *att_des,
+ const struct GainsOrder3Vect3 *k_att_rm, const struct AttQuat *bounds, struct AttQuat *att_ref);
+static void generate_reference_thrust(float dt, float thrust_des, const float k_thrust_rm,
+ const struct ThrustRef *bounds_min, const struct ThrustRef *bounds_max, struct ThrustRef *thrust_ref);
+static struct FloatVect3 control_error_rate(const struct AttQuat *att_ref, const struct AttStateQuat *att_state,
+ const struct GainsOrder2Vect3 *k_rate_ec) __attribute__((unused));
+static struct FloatVect3 control_error_attitude(const struct AttQuat *att_ref, const struct AttStateQuat *att_state,
+ const struct GainsOrder3Vect3 *k_att_ec);
+static float control_error_thrust(const struct ThrustRef *thrust_ref, const float thrust_state,
+ const float k_thrust_ec);
+static void compute_wls_upper_bounds(float u_d_max[ANDI_NUM_ACT], const float act_state[ANDI_NUM_ACT],
+ const float act_max[ANDI_NUM_ACT], const float act_rate_max[ANDI_NUM_ACT],
+ const float actuator_bandwidth[ANDI_NUM_ACT]);
+static void compute_wls_lower_bounds(float u_d_min[ANDI_NUM_ACT], const float act_state[ANDI_NUM_ACT],
+ const float act_min[ANDI_NUM_ACT], const float act_rate_min[ANDI_NUM_ACT],
+ const float actuator_bandwidth[ANDI_NUM_ACT]);
+static void compute_wls_u_scaler(float u_scaler[ANDI_NUM_ACT], const float act_min[ANDI_NUM_ACT],
+ const float act_max[ANDI_NUM_ACT]);
+static void compute_wls_v_scaler(float v_scaler[ANDI_NUM_ACT], const float v[ANDI_NUM_ACT]) __attribute__((unused));
+
+static inline float ec_k1_order3_f(const float omega_n, const float zeta, const float omega_a) { return (omega_n * omega_n * (omega_a - 2 * zeta * omega_n)); }
+static inline float ec_k2_order3_f(const float omega_n, const float zeta, const float omega_a) { return (omega_n * omega_n + 2.0f * zeta * omega_n * (omega_a - 2 * zeta * omega_n)); }
+static inline float ec_k3_order3_f(const float omega_n __attribute__((unused)),
+ const float zeta __attribute__((unused)), const float omega_a) { return omega_a; }
+static inline float rm_k1_order3_f(const float omega_n, const float zeta, const float omega_a) { return (omega_n * omega_n * omega_a) / (omega_n * omega_n + 2 * zeta * omega_n * omega_a); }
+static inline float rm_k2_order3_f(const float omega_n, const float zeta, const float omega_a) { return (omega_n * omega_n + 2 * zeta * omega_n * omega_a) / (2 * zeta * omega_n + omega_a); }
+static inline float rm_k3_order3_f(const float omega_n, const float zeta, const float omega_a) { return 2 * zeta * omega_n + omega_a; }
+
+static inline float ec_k1_order2_f(const float omega_a, const float zeta) { return omega_a * omega_a / (4 * zeta * zeta); }
+static inline float ec_k2_order2_f(const float omega_a, const float zeta __attribute__((unused))) { return omega_a; }
+static inline float rm_k1_order2_f(const float omega_a, const float zeta) { return ec_k2_order2_f(omega_a, zeta) / ec_k2_order2_f(omega_a, zeta); }
+static inline float rm_k2_order2_f(const float omega_a, const float zeta) { return ec_k2_order2_f(omega_a, zeta); }
+
+struct PolesOrder2Vect3 andi_p_rate_ec = {
+ .omega_a = {
+ .x = STABILIZATION_ANDI_POLE_RATE_EC_OMEGA_A_X,
+ .y = STABILIZATION_ANDI_POLE_RATE_EC_OMEGA_A_Y,
+ .z = STABILIZATION_ANDI_POLE_RATE_EC_OMEGA_A_Z
+ },
+ .zeta = {
+ .x = STABILIZATION_ANDI_POLE_RATE_EC_ZETA_X,
+ .y = STABILIZATION_ANDI_POLE_RATE_EC_ZETA_Y,
+ .z = STABILIZATION_ANDI_POLE_RATE_EC_ZETA_Z
+ }
+};
+struct PolesOrder2Vect3 andi_p_rate_rm = {
+ .omega_a = {
+ .x = STABILIZATION_ANDI_POLE_RATE_RM_OMEGA_A_X,
+ .y = STABILIZATION_ANDI_POLE_RATE_RM_OMEGA_A_Y,
+ .z = STABILIZATION_ANDI_POLE_RATE_RM_OMEGA_A_Z
+ },
+ .zeta = {
+ .x = STABILIZATION_ANDI_POLE_RATE_RM_ZETA_X,
+ .y = STABILIZATION_ANDI_POLE_RATE_RM_ZETA_Y,
+ .z = STABILIZATION_ANDI_POLE_RATE_RM_ZETA_Z
+ }
+};
+struct PolesOrder3Vect3 andi_p_att_ec = {
+ .omega_n = {
+ .x = STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_N_X,
+ .y = STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_N_Y,
+ .z = STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_N_Z
+ },
+ .zeta = {
+ .x = STABILIZATION_ANDI_POLE_ATT_EC_ZETA_X,
+ .y = STABILIZATION_ANDI_POLE_ATT_EC_ZETA_Y,
+ .z = STABILIZATION_ANDI_POLE_ATT_EC_ZETA_Z
+ },
+ .omega_a = {
+ .x = STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_A_X,
+ .y = STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_A_Y,
+ .z = STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_A_Z
+ }
+};
+struct PolesOrder3Vect3 andi_p_att_rm = {
+ .omega_n = {
+ .x = STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_N_X,
+ .y = STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_N_Y,
+ .z = STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_N_Z
+ },
+ .zeta = {
+ .x = STABILIZATION_ANDI_POLE_ATT_RM_ZETA_X,
+ .y = STABILIZATION_ANDI_POLE_ATT_RM_ZETA_Y,
+ .z = STABILIZATION_ANDI_POLE_ATT_RM_ZETA_Z
+ },
+ .omega_a = {
+ .x = STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_A_X,
+ .y = STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_A_Y,
+ .z = STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_A_Z
+ }
+};
+float andi_p_thrust_ec = STABILIZATION_ANDI_POLE_THRUST_EC;
+float andi_p_thrust_rm = STABILIZATION_ANDI_POLE_THRUST_RM;
+
+
+// WLS allocation variables
+struct WLS_t wls_stab_p = {
+ .nu = ANDI_NUM_ACT,
+ .nv = ANDI_OUTPUTS,
+ .gamma_sq = 100000.0,
+ .u_pref = {0.0f},
+ .u_min = {0.0f},
+ .u_max = {0.0f},
+ .PC = 0.0f,
+ .SC = 0.0f,
+ .iter = 0
+};
+
+// Controller gains
+struct GainsOrder2Vect3 andi_k_rate_ec;
+struct GainsOrder2Vect3 andi_k_rate_rm;
+struct GainsOrder3Vect3 andi_k_att_ec;
+struct GainsOrder3Vect3 andi_k_att_rm;
+float andi_k_thrust_ec;
+float andi_k_thrust_rm;
+
+// Complementary filter instances for state dependent contribution
+struct FirstOrderComplementaryVect3 attitude_rates_cf;
+struct Butterworth2ComplementaryVect3 attitude_accel_cf;
+struct FloatRates angular_rates_obm;
+
+struct FirstOrderComplementaryVect3 linear_vel_cf;
+struct Butterworth2ComplementaryVect3 linear_accel_cf;
+struct FloatVect3 linear_velocity_obm;
+
+// Actuator and delay
+struct FirstOrderZOHLowPass actuator_obm_zohlpf[ANDI_NUM_ACT]; // ZOH low pass filter for actuator model
+struct TransportDelay actuator_obm_delay[ANDI_NUM_ACT]; // transport delay for actuator model
+float actuator_state[ANDI_NUM_ACT]; // from feedback or model
+float actuator_state_lpf[ANDI_NUM_ACT]; // delayed actuator state
+
+// T4 Actuator feedback handling
+struct ActuatorsT4In actuators_t4_obs;
+abi_event actuators_t4_in_event;
+float actuator_meas[ANDI_NUM_ACT]; // measured actuator feedback
+
+// Raw state measurement variables
+struct FloatRates rates_prev;
+
+// State variables
+struct AttStateQuat attitude_state_cf; // undelayed attitude state for feedforward
+struct LinState linear_state_cf; // undelayed linear state for feedforward
+float thrust_state; // delayed thrust state for feedback
+
+// Reference model variables
+struct AttQuat attitude_ref;
+struct ThrustRef thrust_ref;
+
+// Setpoints
+struct FloatRates rates_des;
+struct FloatQuat attitude_des;
+float thrust_des;
+
+// Bounds
+struct AttQuat attitude_bounds;
+struct ThrustRef thrust_bounds_min;
+struct ThrustRef thrust_bounds_max;
+
+// Controller variables
+float ce_mat[ANDI_NUM_ACT * ANDI_OUTPUTS];
+float du_min[ANDI_NUM_ACT];
+float du_max[ANDI_NUM_ACT];
+float du_cmd[ANDI_NUM_ACT];
+float u_cmd[ANDI_NUM_ACT];
+
+// Pseudo command variables
+float nu_obj[ANDI_OUTPUTS]; // Total pseudo command allocated to the actuators
+float nu_ec[ANDI_OUTPUTS]; // Pseudo command from the error controller
+float nu_obm[ANDI_OUTPUTS]; // Pseudo command from the on board model state dependent term
+// float nu_reconstructed[ANDI_OUTPUTS]; // Reconstructed angular acceleration from the actuator commands (for model verification)
+
+#if PERIODIC_TELEMETRY
+#include "modules/datalink/telemetry.h"
+static void send_wls_v_stabilization_andi(struct transport_tx *trans, struct link_device *dev)
+{
+ char *name = "andi";
+ pprz_msg_send_WLS_V(trans, dev, AC_ID,
+ strlen(name), name,
+ &wls_stab_p.gamma_sq, // Does this need scaling as a function of scaling factor?
+ (uint8_t *)&wls_stab_p.iter,
+ ANDI_OUTPUTS, nu_obj,
+ ANDI_OUTPUTS, (float *)WLS_WV);
+}
+
+static void send_wls_u_stabilization_andi(struct transport_tx *trans, struct link_device *dev)
+{
+ char *name = "andi";
+ float zero_array[ANDI_NUM_ACT] = {0.0f};
+ pprz_msg_send_WLS_U(trans, dev, AC_ID,
+ strlen(name), name,
+ ANDI_NUM_ACT, (float *)WLS_WU,
+ ANDI_NUM_ACT, zero_array,
+ ANDI_NUM_ACT, du_min,
+ ANDI_NUM_ACT, du_max,
+ ANDI_NUM_ACT, du_cmd);
+}
+
+static void send_eff_mat_stabilization_andi(struct transport_tx *trans, struct link_device *dev)
+{
+ float zero = 0.0f;
+ pprz_msg_send_EFF_MAT_STAB(trans, dev, AC_ID,
+ ANDI_NUM_ACT, &ce_mat[0 * ANDI_NUM_ACT],
+ ANDI_NUM_ACT, &ce_mat[1 * ANDI_NUM_ACT],
+ ANDI_NUM_ACT, &ce_mat[2 * ANDI_NUM_ACT],
+ ANDI_NUM_ACT, &ce_mat[3 * ANDI_NUM_ACT],
+ 1, &zero);
+}
+
+static void send_stab_attitude_stabilization_andi(struct transport_tx *trans, struct link_device *dev)
+{
+ pprz_msg_send_STAB_ATTITUDE(trans, dev, AC_ID,
+ 4, (float *)&attitude_des,
+ 4, (float *)&attitude_state_cf.att,
+ 4, (float *)&attitude_ref.att,
+ 3, (float *)&attitude_state_cf.att_d,
+ 3, (float *)&attitude_ref.att_d,
+ 3, (float *)&attitude_state_cf.att_2d,
+ 3, (float *)&attitude_ref.att_2d,
+ 3, (float *)&attitude_ref.att_3d,
+ ANDI_OUTPUTS, actuator_state);
+}
+// For debug only
+// static void send_stab_thrust_stabilization_andi(struct transport_tx *trans, struct link_device *dev)
+// {
+// pprz_msg_send_STAB_THRUST(trans, dev, AC_ID,
+// &thrust_des,
+// &thrust_ref.thrust,
+// &thrust_state,
+// &thrust_ref.thrust_d);
+// }
+
+// static void send_on_board_model_stabilization_andi(struct transport_tx *trans, struct link_device *dev)
+// {
+// pprz_msg_send_ON_BOARD_MODEL(trans, dev, AC_ID,
+// 3, (float *)&linear_state_cf.vel,
+// 3, (float *)&linear_state_cf.acc,
+// ANDI_NUM_ACT, (float *)&actuator_meas,
+// ANDI_OUTPUTS, (float *)&nu_obm);
+// }
+
+// static void send_stab_pseudo_command_stabilization_andi(struct transport_tx *trans, struct link_device *dev)
+// {
+// /**
+// * nu_obj: Total pseudo command sent to the actuators
+// * nu_ec: Pseudo command from the error controller
+// * nu_obm: Pseudo command from the on board model state dependent term
+// * nu_reconstructed: Reconstructed angular acceleration from the actuator commands.
+// * This is used to verify the on board model. If the OBM is perfect, then nu_reconstructed should be
+// * equal to the measured state plus any external disturbances.
+// *
+// * nu_obj = nu_ec + nu_obm
+// * nu_reconstructed = Ce * u_meas - nu_obm NOTE: This is wrong equation, fix it.
+// *
+// * FIXME: nu_reconstructed has been repurposed and no longer is the reconstructed value. Update the telemetry message accordingly.
+// */
+// pprz_msg_send_STAB_PSEUDO_COMMAND(trans, dev, AC_ID,
+// ANDI_OUTPUTS, nu_obj, // Total pseudo command
+// ANDI_OUTPUTS, nu_ec, // From error controller
+// ANDI_OUTPUTS, nu_obm, // From on board model state dependent term
+// ANDI_OUTPUTS, nu_reconstructed); // Reconstructed from actuator commands
+// }
+
+#endif // PERIODIC_TELEMETRY
+
+static void actuators_t4_in_callback(uint8_t sender_id __attribute__((unused)),
+ struct ActuatorsT4In *actuators_t4_in_ptr, float *actuators_t4_extra_data_in_ptr __attribute__((unused)));
+
+/**
+ * @brief Callback function to handle incoming actuator telemetry data.
+ *
+ * This function copies the data from the input struct pointer `actuators_t4_in_ptr`
+ * into a local static instance `actuator_obs` for further processing or monitoring.
+ *
+ * @param sender_id Identifier of the sender of the telemetry data.
+ * @param actuators_t4_in_ptr Pointer to the incoming ActuatorsT4In struct containing
+ * ESC telemetry, servo angles, loads, and other actuator info.
+ * @param actuators_t4_extra_data_in_ptr Pointer to additional extra actuator data (unused).
+ */
+static void actuators_t4_in_callback(uint8_t sender_id __attribute__((unused)),
+ struct ActuatorsT4In *actuators_t4_in_ptr, float *actuators_t4_extra_data_in_ptr __attribute__((unused)))
+{
+ // Copy the entire ActuatorsT4In struct from the pointer to actuator_obs
+ memcpy(&actuators_t4_obs, actuators_t4_in_ptr, sizeof(struct ActuatorsT4In));
+}
+
+/**
+ * @brief Extract actuator states from ActuatorsT4In struct.
+ *
+ * This function converts selected actuator telemetry data from the
+ * input struct into floating-point values representing angles in radians
+ * and rotational speeds in radians per second, storing them in the
+ * provided actuator_state array.
+ *
+ * FIXME: Avoid hardcoding indices, conversion factors, and inversions; This requires additional functionality in actuators_t4.
+ * FIXME: Put actuator feedback / state management in a separate module.
+ * FIXME: All ESC type actuators will be squared rpm for thrust linearization.
+ *
+ * @param[out] actuator_state Array of floats with size ANDI_NUM_ACT where the
+ * converted actuator states will be stored. The caller
+ * must allocate this.
+ * @param[in] actuators_t4_in_ptr Pointer to the input struct containing actuator telemetry,
+ * including servo angles (in 1e-2 degrees) and ESC RPM.
+ */
+static void fetch_actuators_t4(float actuator_meas[ANDI_NUM_ACT], const struct ActuatorsT4In *actuators_t4_in_ptr)
+{
+ actuator_meas[0] = RadOfCentiDeg((float)actuators_t4_in_ptr->servo_1_angle);
+ actuator_meas[1] = -RadOfCentiDeg((float)actuators_t4_in_ptr->servo_6_angle);
+ actuator_meas[2] = (float)actuators_t4_in_ptr->esc_1_rpm * 2 * M_PI / 60; // Convert rpm to rad/s
+ actuator_meas[2] *= actuator_meas[2]; // square motor rpm
+ actuator_meas[3] = (float)actuators_t4_in_ptr->esc_2_rpm * 2 * M_PI / 60;
+ actuator_meas[3] *= actuator_meas[3]; // square motor rpm
+}
+
+/**
+ * Compute reference-model gains for a 3rd-order 3D system.
+ * Each axis gain (x, y, z) is computed using rm_k*_order3_f() with omega_n, zeta, and p1 parameters.
+ * @param[in] poles Pointer to PolesOrder3Vect3 containing omega_n, zeta, and p1 for each axis.
+ * @return Struct containing k1, k2, k3 gains for x, y, z.
+ */
+static struct GainsOrder3Vect3 compute_reference_gains_order_3_vect_3(const struct PolesOrder3Vect3 *poles)
+{
+ struct GainsOrder3Vect3 gains;
+ gains.k1.x = rm_k1_order3_f(poles->omega_n.x, poles->zeta.x, poles->omega_a.x);
+ gains.k1.y = rm_k1_order3_f(poles->omega_n.y, poles->zeta.y, poles->omega_a.y);
+ gains.k1.z = rm_k1_order3_f(poles->omega_n.z, poles->zeta.z, poles->omega_a.z);
+
+ gains.k2.x = rm_k2_order3_f(poles->omega_n.x, poles->zeta.x, poles->omega_a.x);
+ gains.k2.y = rm_k2_order3_f(poles->omega_n.y, poles->zeta.y, poles->omega_a.y);
+ gains.k2.z = rm_k2_order3_f(poles->omega_n.z, poles->zeta.z, poles->omega_a.z);
+
+ gains.k3.x = rm_k3_order3_f(poles->omega_n.x, poles->zeta.x, poles->omega_a.x);
+ gains.k3.y = rm_k3_order3_f(poles->omega_n.y, poles->zeta.y, poles->omega_a.y);
+ gains.k3.z = rm_k3_order3_f(poles->omega_n.z, poles->zeta.z, poles->omega_a.z);
+ return gains;
+}
+
+/**
+ * Compute reference-model gains for a 2nd-order 3D system.
+ * Each axis gain (x, y, z) is computed using rm_k*_order2_f() with omega_n and zeta parameters.
+ * @param[in] poles Pointer to PolesOrder2Vect3 containing omega_n and zeta for each axis.
+ * @return Struct containing k1, k2 gains for x, y, z.
+ */
+static struct GainsOrder2Vect3 compute_reference_gains_order_2_vect_3(const struct PolesOrder2Vect3 *poles)
+{
+ struct GainsOrder2Vect3 gains;
+ gains.k1.x = rm_k1_order2_f(poles->omega_a.x, poles->zeta.x);
+ gains.k1.y = rm_k1_order2_f(poles->omega_a.y, poles->zeta.y);
+ gains.k1.z = rm_k1_order2_f(poles->omega_a.z, poles->zeta.z);
+
+ gains.k2.x = rm_k2_order2_f(poles->omega_a.x, poles->zeta.x);
+ gains.k2.y = rm_k2_order2_f(poles->omega_a.y, poles->zeta.y);
+ gains.k2.z = rm_k2_order2_f(poles->omega_a.z, poles->zeta.z);
+ return gains;
+}
+
+/**
+ * Compute error-compensation gains for a 3rd-order 3D system.
+ * Each axis gain (x, y, z) is computed using ec_k*_order3_f() with omega_n, zeta, and p1 parameters.
+ * @param[in] poles Pointer to PolesOrder3Vect3 containing omega_n, zeta, and p1 for each axis.
+ * @return Struct containing k1, k2, k3 gains for x, y, z.
+ */
+static struct GainsOrder3Vect3 compute_error_gains_order_3_vect_3(const struct PolesOrder3Vect3 *poles)
+{
+ struct GainsOrder3Vect3 gains;
+ gains.k1.x = ec_k1_order3_f(poles->omega_n.x, poles->zeta.x, poles->omega_a.x);
+ gains.k1.y = ec_k1_order3_f(poles->omega_n.y, poles->zeta.y, poles->omega_a.y);
+ gains.k1.z = ec_k1_order3_f(poles->omega_n.z, poles->zeta.z, poles->omega_a.z);
+
+ gains.k2.x = ec_k2_order3_f(poles->omega_n.x, poles->zeta.x, poles->omega_a.x);
+ gains.k2.y = ec_k2_order3_f(poles->omega_n.y, poles->zeta.y, poles->omega_a.y);
+ gains.k2.z = ec_k2_order3_f(poles->omega_n.z, poles->zeta.z, poles->omega_a.z);
+
+ gains.k3.x = ec_k3_order3_f(poles->omega_n.x, poles->zeta.x, poles->omega_a.x);
+ gains.k3.y = ec_k3_order3_f(poles->omega_n.y, poles->zeta.y, poles->omega_a.y);
+ gains.k3.z = ec_k3_order3_f(poles->omega_n.z, poles->zeta.z, poles->omega_a.z);
+ return gains;
+}
+
+/**
+ * Compute error-compensation gains for a 2nd-order 3D system.
+ * Each axis gain (x, y, z) is computed using ec_k*_order2_f() with omega_n and zeta parameters.
+ * @param[in] poles Pointer to PolesOrder2Vect3 containing omega_n and zeta for each axis.
+ * @return Struct containing k1, k2 gains for x, y, z.
+ */
+static struct GainsOrder2Vect3 compute_error_gains_order_2_vect_3(const struct PolesOrder2Vect3 *poles)
+{
+ struct GainsOrder2Vect3 gains;
+ gains.k1.x = ec_k1_order2_f(poles->omega_a.x, poles->zeta.x);
+ gains.k1.y = ec_k1_order2_f(poles->omega_a.y, poles->zeta.y);
+ gains.k1.z = ec_k1_order2_f(poles->omega_a.z, poles->zeta.z);
+
+ gains.k2.x = ec_k2_order2_f(poles->omega_a.x, poles->zeta.x);
+ gains.k2.y = ec_k2_order2_f(poles->omega_a.y, poles->zeta.y);
+ gains.k2.z = ec_k2_order2_f(poles->omega_a.z, poles->zeta.z);
+ return gains;
+}
+
+/**
+ * @brief Generates a bounded second-order reference signal for attitude rate control.
+ *
+ * This function computes smooth angular rate, acceleration, and jerk references based on the desired rates,
+ * applying bounded limits to ensure stability and safe dynamic behavior. It updates the reference states
+ * in place within the provided `att_ref` structure, which is a quaternion-based reference with associated rate states.
+ *
+ * @param[in] dt The sampling interval in seconds.
+ * @param[in] rate_des Desired angular rates (p, q, r) as a `FloatRates` structure.
+ * @param[in] k_rate_rm Gain parameters as `GainsOrder2Vect3`, for the proportional and derivative terms.
+ * @param[in] bounds Limits for the desired rates, rates derivatives, and accelerations, in `AttRefEulers`.
+ * @param[in,out] att_ref The reference model states (attitude quaternion, rates, and derivatives),
+ * updated in place to produce the reference command.
+ */
+static void generate_reference_rate(
+ float dt,
+ const struct FloatRates *rate_des,
+ const struct GainsOrder2Vect3 *k_rate_rm,
+ const struct AttQuat *bounds,
+ struct AttQuat *att_ref)
+{
+ float p_des = rate_des->p;
+ float q_des = rate_des->q;
+ float r_des = rate_des->r;
+
+ BoundAbs(p_des, bounds->att_d.p);
+ BoundAbs(q_des, bounds->att_d.q);
+ BoundAbs(r_des, bounds->att_d.r);
+
+ float p_d_des = k_rate_rm->k1.x * (p_des - att_ref->att_d.p);
+ float q_d_des = k_rate_rm->k1.y * (q_des - att_ref->att_d.q);
+ float r_d_des = k_rate_rm->k1.z * (r_des - att_ref->att_d.r);
+
+ BoundAbs(p_d_des, bounds->att_2d.x);
+ BoundAbs(q_d_des, bounds->att_2d.y);
+ BoundAbs(r_d_des, bounds->att_2d.z);
+
+ float p_2d_des = k_rate_rm->k2.x * (p_d_des - att_ref->att_2d.x);
+ float q_2d_des = k_rate_rm->k2.y * (q_d_des - att_ref->att_2d.y);
+ float r_2d_des = k_rate_rm->k2.z * (r_d_des - att_ref->att_2d.z);
+
+ BoundAbs(p_2d_des, bounds->att_3d.x);
+ BoundAbs(q_2d_des, bounds->att_3d.y);
+ BoundAbs(r_2d_des, bounds->att_3d.z);
+
+ att_ref->att_3d.x = p_2d_des;
+ att_ref->att_3d.y = q_2d_des;
+ att_ref->att_3d.z = r_2d_des;
+
+ att_ref->att_2d.x += p_2d_des * dt;
+ att_ref->att_2d.y += q_2d_des * dt;
+ att_ref->att_2d.z += r_2d_des * dt;
+
+ att_ref->att_d.p += att_ref->att_2d.x * dt;
+ att_ref->att_d.q += att_ref->att_2d.y * dt;
+ att_ref->att_d.r += att_ref->att_2d.z * dt;
+
+ float_quat_identity(&att_ref->att);
+}
+
+/**
+ * @brief Generates a bounded second-order reference signal for quaternion-based attitude control.
+ *
+ * Computes smooth angular rate, acceleration, and jerk references using quaternion error feedback.
+ * Bounds are applied to limit reference values and maintain system stability. The function updates
+ * the reference states in place within the provided `att_ref` structure, which includes quaternion attitude
+ * and associated rate states.
+ *
+ * @param[in] dt Sampling interval in seconds.
+ * @param[in] att_des Desired attitude as a unit quaternion.
+ * @param[in] k_att_rm Gain parameters (proportional and derivative gains for rate control).
+ * @param[in] bounds Limits on rate references and derivatives (angular velocity and higher).
+ * @param[in,out] att_ref Reference model state containing attitude quaternion and rate state, updated in place.
+ *
+ * FIXME: The bounds introduce nonlinearities that may destabilize the reference model for large changes in attitude.
+ * Currently removed, consider revising the bounding strategy.
+ * FIXME: Quaternion error calculation assumes small angle errors; may need to be revised for large attitude changes.
+ */
+static void generate_reference_attitude(
+ float dt,
+ const struct FloatQuat *att_des,
+ const struct GainsOrder3Vect3 *k_att_rm,
+ const struct AttQuat *bounds,
+ struct AttQuat *att_ref)
+{
+ struct FloatQuat att_err; // rotation from ref to des
+ float_quat_inv_comp_norm_shortest(&att_err, &att_ref->att, att_des);
+ float p_des = k_att_rm->k1.x * att_err.qx * 2;
+ float q_des = k_att_rm->k1.y * att_err.qy * 2;
+ float r_des = k_att_rm->k1.z * att_err.qz * 2;
+
+ BoundAbs(p_des, bounds->att_d.p);
+ BoundAbs(q_des, bounds->att_d.q);
+ BoundAbs(r_des, bounds->att_d.r);
+
+ float p_d_des = k_att_rm->k2.x * (p_des - att_ref->att_d.p);
+ float q_d_des = k_att_rm->k2.y * (q_des - att_ref->att_d.q);
+ float r_d_des = k_att_rm->k2.z * (r_des - att_ref->att_d.r);
+
+ BoundAbs(p_d_des, bounds->att_2d.x);
+ BoundAbs(q_d_des, bounds->att_2d.y);
+ BoundAbs(r_d_des, bounds->att_2d.z);
+
+ float p_2d_des = k_att_rm->k3.x * (p_d_des - att_ref->att_2d.x);
+ float q_2d_des = k_att_rm->k3.y * (q_d_des - att_ref->att_2d.y);
+ float r_2d_des = k_att_rm->k3.z * (r_d_des - att_ref->att_2d.z);
+
+ BoundAbs(p_2d_des, bounds->att_3d.x);
+ BoundAbs(q_2d_des, bounds->att_3d.y);
+ BoundAbs(r_2d_des, bounds->att_3d.z);
+
+ att_ref->att_3d.x = p_2d_des;
+ att_ref->att_3d.y = q_2d_des;
+ att_ref->att_3d.z = r_2d_des;
+
+ att_ref->att_2d.x += p_2d_des * dt;
+ att_ref->att_2d.y += q_2d_des * dt;
+ att_ref->att_2d.z += r_2d_des * dt;
+
+ att_ref->att_d.p += att_ref->att_2d.x * dt;
+ att_ref->att_d.q += att_ref->att_2d.y * dt;
+ att_ref->att_d.r += att_ref->att_2d.z * dt;
+
+ float_quat_integrate(&att_ref->att, &att_ref->att_d, dt);
+ float_quat_normalize(&att_ref->att);
+}
+
+
+/**
+ * @brief Generates a bounded second-order reference signal for thrust control.
+ *
+ * Applies bounded limits to a desired thrust input and computes smoothed
+ * thrust rate and integrated thrust references. The reference states are
+ * updated in place inside the provided \p thrust_ref structure.
+ *
+ * @param[in] dt Sampling interval in seconds.
+ * @param[in] thrust_des Desired thrust input value.
+ * @param[in] k_thrust_rm Gain parameter for thrust rate control (proportional gain).
+ * @param[in] bounds Maximum allowable thrust magnitude.
+ * @param[in,out] thrust_ref Pointer to ThrustRef struct holding thrust reference states.
+ */
+static void generate_reference_thrust(
+ float dt,
+ float thrust_des,
+ const float k_thrust_rm,
+ const struct ThrustRef *bounds_min,
+ const struct ThrustRef *bounds_max,
+ struct ThrustRef *thrust_ref)
+{
+ Bound(thrust_des, bounds_min->thrust, bounds_max->thrust);
+ thrust_ref->thrust_d = k_thrust_rm * (thrust_des - thrust_ref->thrust);
+
+ Bound(thrust_ref->thrust_d, bounds_min->thrust_d, bounds_max->thrust_d);
+ thrust_ref->thrust += thrust_ref->thrust_d * dt;
+}
+
+/**
+ * @brief Computes the control error command for attitude rate regulation.
+ *
+ * This function calculates the virtual control input vector \c nu required to reduce the error
+ * between the reference and current angular rate states. It uses proportional and derivative gains
+ * on the differences of rate and rate derivative components, respectively, and adds feedforward jerk.
+ *
+ * @param[in] att_ref Pointer to the reference attitude and rate state structure.
+ * @param[in] att_state Pointer to the current attitude and rate state structure.
+ * @param[in] k_rate_ec Pointer to gain parameters structure, containing proportional and derivative gains.
+ *
+ * @return A \c FloatVect3 structure representing the computed virtual control input vector.
+ */
+static struct FloatVect3 control_error_rate(
+ const struct AttQuat *att_ref,
+ const struct AttStateQuat *att_state,
+ const struct GainsOrder2Vect3 *k_rate_ec)
+{
+ struct FloatVect3 nu = att_ref->att_3d;
+
+ nu.x += k_rate_ec->k2.x * (att_ref->att_2d.x - att_state->att_2d.x);
+ nu.y += k_rate_ec->k2.y * (att_ref->att_2d.y - att_state->att_2d.y);
+ nu.z += k_rate_ec->k2.z * (att_ref->att_2d.z - att_state->att_2d.z);
+
+ nu.x += k_rate_ec->k1.x * (att_ref->att_d.p - att_state->att_d.p);
+ nu.y += k_rate_ec->k1.y * (att_ref->att_d.q - att_state->att_d.q);
+ nu.z += k_rate_ec->k1.z * (att_ref->att_d.r - att_state->att_d.r);
+
+ return nu;
+}
+
+/**
+ * @brief Computes the attitude rate error control command.
+ *
+ * This function calculates the control input vector \c nu to correct the attitude error and
+ * the associated angular rate errors using proportional-derivative gains. The quaternion error
+ * between the reference and current attitudes is also factored in the control law.
+ *
+ * @param[in] att_ref Pointer to the reference attitude and rate states (quaternion-based).
+ * @param[in] att_state Pointer to the current attitude and rate states.
+ * @param[in] k_att_ec Pointer to gain parameters struct containing proportional, derivative, and jerk gains.
+ *
+ * @return The computed virtual control input vector.
+ */
+static struct FloatVect3 control_error_attitude(
+ const struct AttQuat *att_ref,
+ const struct AttStateQuat *att_state,
+ const struct GainsOrder3Vect3 *k_att_ec)
+{
+ struct FloatVect3 nu = att_ref->att_3d;
+
+ nu.x += k_att_ec->k3.x * (att_ref->att_2d.x - att_state->att_2d.x);
+ nu.y += k_att_ec->k3.y * (att_ref->att_2d.y - att_state->att_2d.y);
+ nu.z += k_att_ec->k3.z * (att_ref->att_2d.z - att_state->att_2d.z);
+
+ nu.x += k_att_ec->k2.x * (att_ref->att_d.p - att_state->att_d.p);
+ nu.y += k_att_ec->k2.y * (att_ref->att_d.q - att_state->att_d.q);
+ nu.z += k_att_ec->k2.z * (att_ref->att_d.r - att_state->att_d.r);
+
+ struct FloatQuat att_err; // rotation from state to reference
+ float_quat_inv_comp_norm_shortest(&att_err, &att_state->att, &att_ref->att);
+ // Multiplication by 2 needed to convert quaternion difference to angular rate.
+ nu.x += k_att_ec->k1.x * att_err.qx * 2;
+ nu.y += k_att_ec->k1.y * att_err.qy * 2;
+ nu.z += k_att_ec->k1.z * att_err.qz * 2;
+
+ return nu;
+}
+
+/**
+ * @brief Computes the thrust control command based on desired and current thrust.
+ *
+ * This function calculates a corrected thrust command using a simple proportional
+ * feedback law. The correction term is scaled by the thrust error gain k_thrust_ec
+ * to reduce the difference between the desired thrust (thrust_ref->thrust)
+ * and the current thrust (thrust_state). The resulting command is based on
+ * the desired thrust feedforward input thrust_ref->thrust_d.
+ *
+ * @param[in] thrust_ref Pointer to a structure containing the desired thrust values.
+ * @param[in] thrust_state Current measured thrust value.
+ * @param[in] k_thrust_ec Proportional gain applied to the thrust error correction.
+ *
+ * @return The computed virtual control thrust input.
+ */
+static float control_error_thrust(
+ const struct ThrustRef *thrust_ref,
+ const float thrust_state,
+ const float k_thrust_ec)
+{
+ float nu = thrust_ref->thrust_d;
+ nu += k_thrust_ec * (thrust_ref->thrust - thrust_state);
+ return nu;
+}
+
+/**
+ * @brief Compute upper bounds for actuator rate commands based on actuator state and constraints.
+ *
+ * This function calculates the maximum allowable actuator rate commands (`u_d_max`) for each actuator to ensure that:
+ * 1. The actuator position does not exceed its specified maximum (`act_max`) in the next timestep.
+ * 2. The actuator rate does not exceed its maximum allowed rate (`act_rate_max`).
+ * The computed upper bound is the minimum of the position-based rate limit and the maximum rate limit.
+ *
+ * @param[out] u_d_max Array to store the computed upper bounds for actuator rates.
+ * @param[in] act_state Current states (positions) of the actuators.
+ * @param[in] act_max Maximum allowable positions for the actuators.
+ * @param[in] act_rate_max Maximum allowable rates for the actuators.
+ * @param[in] actuator_bandwidth Bandwidth of the actuators used to compute rate limits.
+ */
+static void compute_wls_upper_bounds(float u_d_max[ANDI_NUM_ACT], const float act_state[ANDI_NUM_ACT],
+ const float act_max[ANDI_NUM_ACT], const float act_rate_max[ANDI_NUM_ACT], const float actuator_bandwidth[ANDI_NUM_ACT])
+{
+ for (uint8_t i = 0; i < ANDI_NUM_ACT; i++) {
+ // Calculate max rate allowed to avoid exceeding actuator max position in one timestep
+ float rate_limit_pos = (act_max[i] - act_state[i]) * actuator_bandwidth[i];
+ u_d_max[i] = (rate_limit_pos < act_rate_max[i]) ? rate_limit_pos : act_rate_max[i];
+ Bound(u_d_max[i], 0.0f, INFINITY);
+ }
+}
+
+/**
+ * @brief Compute lower bounds for actuator rate commands based on actuator state and constraints.
+ *
+ * This function calculates the minimum allowable actuator rate commands (`u_d_min`) for each actuator to ensure that:
+ * 1. The actuator position does not go below its specified minimum (`act_min`) in the next timestep.
+ * 2. The actuator rate does not go below its minimum allowed rate (`act_rate_min`).
+ * The computed lower bound is the maximum of the position-based rate limit and the minimum rate limit.
+ * Additionally, if the computed lower bound is greater than zero (i.e., actuator cannot reverse direction), it is clamped to zero.
+ *
+ * @param[out] u_d_min Array to store the computed lower bounds for actuator rates.
+ * @param[in] act_state Current states (positions) of the actuators.
+ * @param[in] act_min Minimum allowable positions for the actuators.
+ * @param[in] act_rate_min Minimum allowable rates for the actuators.
+ * @param[in] actuator_bandwidth Bandwidth of the actuators used to compute rate limits.
+ */
+static void compute_wls_lower_bounds(float u_d_min[ANDI_NUM_ACT], const float act_state[ANDI_NUM_ACT],
+ const float act_min[ANDI_NUM_ACT], const float act_rate_min[ANDI_NUM_ACT], const float actuator_bandwidth[ANDI_NUM_ACT])
+{
+ for (uint8_t i = 0; i < ANDI_NUM_ACT; i++) {
+ // Calculate min rate allowed to avoid going below actuator min position in one timestep
+ float rate_limit_pos = (act_min[i] - act_state[i]) * actuator_bandwidth[i];
+ u_d_min[i] = (rate_limit_pos > act_rate_min[i]) ? rate_limit_pos : act_rate_min[i];
+ Bound(u_d_min[i], -INFINITY, 0.0f);
+ }
+}
+
+/**
+ * @brief Compute input scaling factors for normalizing weighted least squares.
+ *
+ * This function calculates the input scaling factors (`u_scaler`) for each actuator
+ * based on the provided minimum (`act_min`) and maximum (`act_max`) actuator values.
+ * The scaling factor is computed as the inverse of the range (max - min). If the
+ * range is zero, the scaling factor is set to 1.0 to avoid division by zero.
+ *
+ * u_norm = u_scaler * u
+ *
+ * @param[out] u_scaler Array to store the computed input scaling factors.
+ * @param[in] act_min Array of minimum actuator values.
+ * @param[in] act_max Array of maximum actuator values.
+ */
+static void compute_wls_u_scaler(float u_scaler[ANDI_NUM_ACT], const float act_min[ANDI_NUM_ACT],
+ const float act_max[ANDI_NUM_ACT])
+{
+ for (uint8_t i = 0; i < ANDI_NUM_ACT; i++) {
+ float range = fabs(act_max[i] - act_min[i]);
+ if (range <= FLT_EPSILON) {
+ u_scaler[i] = 1.0f;
+ } else {
+ u_scaler[i] = 1.0f / range;
+ }
+ }
+}
+
+/**
+ * @brief Compute output scaling factors for normalizing weighted least squares outputs.
+ *
+ * For each output i this sets v_scaler[i] = 1.0f / abs(v[i]) when v[i] is non-zero; otherwise v_scaler[i] is set
+ * to 1.0f to avoid a division-by-zero. The normalized output used in WLS is then
+ *
+ * v_norm = v_scaler * v.
+ *
+ * @param[out] v_scaler Array of length ANDI_NUM_ACT to store the computed inverse scaling factors.
+ * @param[in] v Array of length ANDI_NUM_ACT containing reference/scaling values used to compute the inverse.
+ */
+static void compute_wls_v_scaler(float v_scaler[ANDI_NUM_ACT], const float v[ANDI_NUM_ACT])
+{
+ for (uint8_t i = 0; i < ANDI_NUM_ACT; i++) {
+ if (fabs(v[i]) <= FLT_EPSILON) {
+ v_scaler[i] = 1.0f;
+ } else {
+ v_scaler[i] = 1.0f / fabs(v[i]);
+ }
+ }
+}
+
+void stabilization_andi_init(void)
+{
+ // Compute gains
+ andi_k_rate_ec = compute_error_gains_order_2_vect_3(&andi_p_rate_ec);
+ andi_k_rate_rm = compute_reference_gains_order_2_vect_3(&andi_p_rate_rm);
+ andi_k_att_ec = compute_error_gains_order_3_vect_3(&andi_p_att_ec);
+ andi_k_att_rm = compute_reference_gains_order_3_vect_3(&andi_p_att_rm);
+ andi_k_thrust_ec = andi_p_thrust_ec;
+ andi_k_thrust_rm = andi_p_thrust_rm;
+
+ // Initialize state variables
+ rates_prev.p = 0.0f;
+ rates_prev.q = 0.0f;
+ rates_prev.r = 0.0f;
+
+ float_quat_identity(&attitude_state_cf.att);
+ attitude_state_cf.att_d.p = 0.0f;
+ attitude_state_cf.att_d.q = 0.0f;
+ attitude_state_cf.att_d.r = 0.0f;
+ attitude_state_cf.att_2d.x = 0.0f;
+ attitude_state_cf.att_2d.y = 0.0f;
+ attitude_state_cf.att_2d.z = 0.0f;
+
+ linear_state_cf.vel.x = 0.0f;
+ linear_state_cf.vel.y = 0.0f;
+ linear_state_cf.vel.z = 0.0f;
+ linear_state_cf.acc.x = 0.0f;
+ linear_state_cf.acc.y = 0.0f;
+ linear_state_cf.acc.z = 0.0f;
+
+ thrust_state = 0.0f;
+ float_vect_zero(actuator_state, ANDI_NUM_ACT);
+ float_vect_zero(u_cmd, ANDI_NUM_ACT);
+ float_vect_zero(du_cmd, ANDI_NUM_ACT);
+ float_vect_zero(nu_obj, ANDI_NUM_ACT);
+ float_vect_zero(nu_ec, ANDI_NUM_ACT);
+ float_vect_zero(nu_obm, ANDI_NUM_ACT);
+ // float_vect_zero(nu_reconstructed, ANDI_NUM_ACT);
+
+ // Initialize reference variables
+ float_quat_identity(&attitude_ref.att);
+ attitude_ref.att_d.p = 0.0f;
+ attitude_ref.att_d.q = 0.0f;
+ attitude_ref.att_d.r = 0.0f;
+ attitude_ref.att_2d.x = 0.0f;
+ attitude_ref.att_2d.y = 0.0f;
+ attitude_ref.att_2d.z = 0.0f;
+ attitude_ref.att_3d.x = 0.0f;
+ attitude_ref.att_3d.y = 0.0f;
+ attitude_ref.att_3d.z = 0.0f;
+
+ thrust_ref.thrust = 0.0f;
+ thrust_ref.thrust_d = 0.0f;
+
+ // FIXME: These bounds should be set via parameters
+ // Initialize attitude bounds (symmetric bounds on abs values)
+ float_quat_identity(&attitude_bounds.att);
+ attitude_bounds.att_d.p = 100.0f;
+ attitude_bounds.att_d.q = 100.0f;
+ attitude_bounds.att_d.r = 25.0f;
+ attitude_bounds.att_2d.x = 100.0f;
+ attitude_bounds.att_2d.y = 100.0f;
+ attitude_bounds.att_2d.z = 20.0f;
+ attitude_bounds.att_3d.x = 100.0f;
+ attitude_bounds.att_3d.y = 100.0f;
+ attitude_bounds.att_3d.z = 100.0f;
+
+ // Limit thrust bounds (asymmetric bounds)
+ thrust_bounds_min.thrust = THRUST_MIN;
+ thrust_bounds_max.thrust = THRUST_MAX;
+ thrust_bounds_min.thrust_d = -1000.0f;
+ thrust_bounds_max.thrust_d = 1000.0f;
+
+ // Initial control effectiveness matrix
+ evaluate_obm_f_stb_u(ce_mat, &attitude_state_cf.att_d, &linear_state_cf.vel, ACTUATOR_PREF);
+
+ // Initialize filters
+ init_first_order_complementary_vect3(&attitude_rates_cf, &CUTOFF_FREQ_OMEGA, SAMPLE_TIME);
+ init_butterworth_2_complementary_vect3(&attitude_accel_cf, &CUTOFF_FREQ_OMEGA_DOT, SAMPLE_TIME);
+ angular_rates_obm.p = 0.0f;
+ angular_rates_obm.q = 0.0f;
+ angular_rates_obm.r = 0.0f;
+
+ init_first_order_complementary_vect3(&linear_vel_cf, &CUTOFF_FREQ_VEL, SAMPLE_TIME);
+ init_butterworth_2_complementary_vect3(&linear_accel_cf, &CUTOFF_FREQ_ACCEL, SAMPLE_TIME);
+ linear_velocity_obm.x = 0.0f;
+ linear_velocity_obm.y = 0.0f;
+ linear_velocity_obm.z = 0.0f;
+
+ // Bind T4 actuator feedback abi message
+ AbiBindMsgACTUATORS_T4_IN(ABI_BROADCAST, &actuators_t4_in_event, actuators_t4_in_callback);
+
+ // Precompute discrete-time actuator dynamics coefficients
+ init_first_order_zoh_low_pass_array(ANDI_NUM_ACT, actuator_obm_zohlpf, ACTUATOR_DYNAMICS, SAMPLE_TIME);
+ init_transport_delay_array(ANDI_NUM_ACT, actuator_obm_delay, ACTUATOR_DELAY, actuator_state);
+
+// Start telemetry
+#if PERIODIC_TELEMETRY
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE, send_stab_attitude_stabilization_andi);
+ // register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_THRUST, send_stab_thrust_stabilization_andi); // For debug only, dont forget to add your message.
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_EFF_MAT_STAB, send_eff_mat_stabilization_andi);
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_WLS_V, send_wls_v_stabilization_andi);
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_WLS_U, send_wls_u_stabilization_andi);
+ // register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_PSEUDO_COMMAND, send_stab_pseudo_command_stabilization_andi);
+ // register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ON_BOARD_MODEL, send_on_board_model_stabilization_andi);
+#endif
+}
+
+/**
+ * @brief Initializes the ANDI stabilization controller state upon entering stabilization mode.
+ *
+ * This function resets all relevant state variables, filters, and actuator states to match the current
+ * measurements when entering the ANDI stabilization mode. It ensures that the controller starts from a
+ * consistent state when entering stabilization.
+ *
+ * NOTE: This function currently assumes that the actuator feedback message is being received.
+ * FIXME: Transient free initialization for the cascaded complementary filters is not implemented.
+ */
+void stabilization_andi_enter(void)
+{
+ // Reset actuator states to current measurements
+ fetch_actuators_t4(actuator_state, &actuators_t4_obs);
+ reset_first_order_zoh_low_pass_array(ANDI_NUM_ACT, actuator_obm_zohlpf, actuator_state);
+ reset_transport_delay_array(ANDI_NUM_ACT, actuator_obm_delay, actuator_state);
+
+ // Clear previous rates
+ rates_prev.p = 0.0f;
+ rates_prev.q = 0.0f;
+ rates_prev.r = 0.0f;
+
+ // Reset state to current measurements, higher order derivatives are set to zero
+ float_quat_vmult(&linear_state_cf.vel, stateGetNedToBodyQuat_f(), (struct FloatVect3 *)stateGetSpeedNed_f());
+ linear_state_cf.acc.x = 0.0f;
+ linear_state_cf.acc.y = 0.0f;
+ linear_state_cf.acc.z = 0.0f;
+ reset_first_order_complementary_vect3(&linear_vel_cf, &linear_state_cf.vel);
+ reset_butterworth_2_complementary_vect3(&linear_accel_cf, &linear_state_cf.acc);
+ linear_velocity_obm = linear_state_cf.vel;
+
+ attitude_state_cf.att = *stateGetNedToBodyQuat_f();
+ attitude_state_cf.att_d = *stateGetBodyRates_f();
+ attitude_state_cf.att_2d.x = 0.0f;
+ attitude_state_cf.att_2d.y = 0.0f;
+ attitude_state_cf.att_2d.z = 0.0f;
+ reset_first_order_complementary_rates(&attitude_rates_cf, &attitude_state_cf.att_d);
+ reset_butterworth_2_complementary_vect3(&attitude_accel_cf, &attitude_state_cf.att_2d);
+ angular_rates_obm = attitude_state_cf.att_d;
+
+ attitude_ref.att = attitude_state_cf.att;
+ attitude_ref.att_d = attitude_state_cf.att_d;
+ attitude_ref.att_2d = attitude_state_cf.att_2d;
+ attitude_ref.att_3d.x = 0.0f;
+ attitude_ref.att_3d.y = 0.0f;
+ attitude_ref.att_3d.z = 0.0f;
+
+ thrust_state = evaluate_obm_thrust_z(actuator_state);
+
+ thrust_ref.thrust = thrust_state;
+ thrust_ref.thrust_d = 0.0f;
+
+ // Reset controller states
+ float_vect_zero(u_cmd, ANDI_NUM_ACT);
+ float_vect_zero(nu_obm, ANDI_OUTPUTS);
+
+ // Initial control effectiveness matrix
+ evaluate_obm_f_stb_u(ce_mat, &attitude_state_cf.att_d, &linear_state_cf.vel, ACTUATOR_PREF);
+
+}
+
+/**
+ * @brief Main ANDI stabilization control loop.
+ *
+ * This function implements the core logic of the ANDI stabilization controller.
+ * The following steps are performed each time the function is called:
+ * 1. Fetch measurements:
+ * - Linear state (velocity and acceleration)
+ * - Attitude state (quaternion, angular rates, angular accelerations)
+ * - Actuator states (from T4 feedback and/or on-board model estimates)
+ *
+ * 2. Filtering:
+ * - Apply cascaded complementary filtering to obtain an undelayed estimates of attitude and linear states. (used for state dependent model contribution (and scheduling))
+ * - Apply low-pass filtering to obtain synchronized (delayed) estimates of attitude and thrust states. (used for control error computation)
+ *
+ * 3. Reference generation:
+ * - Generate reference models for attitude and thrust using desired setpoints and reference model gains.
+ * - Apply low-pass filtering to the references to synchronize with measurement lag.
+ *
+ * 4. Pseudo-command computation:
+ * - Compute error controller pseudo command for attitude and thrust reference and feedback using parallel error controller.
+ * - Compute state dependent on-board model pseudo command contribution based on undelayed state estimates.
+ * - Combine pseudo commands to form the overall virtual control input vector.
+ *
+ * 5. Actuator command allocation:
+ * - Update the control effectiveness matrix based on the current state.
+ * - Compute actuator commands using weighted least squares allocation with bounds and scaling.
+ *
+ * @param[in] use_rate_control Flag indicating whether to use rate control mode.
+ * @param[in] in_flight Flag indicating whether the vehicle is in flight.
+ * @param[in,out] stab_setpoint Pointer to the stabilization setpoint structure.
+ * @param[in,out] thrust_setpoint Pointer to the thrust setpoint structure.
+ * @param[out] cmd Pointer to the output command array for actuators.
+ *
+ * @note The attitude stabilization mode is \c ATTITUDE_HEADING_MODE.
+ * A heading estimate and setpoint is needed. Consider implementing
+ * \c ATTITUDE_HEADING_RATE_MODE to avoid needing a heading estimate.
+ *
+ * FIXME: The function currently recomputes gains at each call. This could be optimized to only recompute when parameters change.
+ * FIXME: The choice of actuator feedback source (T4 vs on-board model) should be configurable.
+ */
+void stabilization_andi_run(bool use_rate_control, bool in_flight, struct StabilizationSetpoint *stab_setpoint,
+ struct ThrustSetpoint *thrust_setpoint, int32_t *cmd)
+{
+ // Recompute gains
+ // FIXME: Only recompute when parameters change, use parameter update handler callback.
+ andi_k_rate_ec = compute_error_gains_order_2_vect_3(&andi_p_rate_ec);
+ andi_k_rate_rm = compute_reference_gains_order_2_vect_3(&andi_p_rate_rm);
+ andi_k_att_ec = compute_error_gains_order_3_vect_3(&andi_p_att_ec);
+ andi_k_att_rm = compute_reference_gains_order_3_vect_3(&andi_p_att_rm);
+ andi_k_thrust_ec = andi_p_thrust_ec;
+ andi_k_thrust_rm = andi_p_thrust_rm;
+
+ // Fetch linear measurements
+ struct LinState lin_meas;
+ float_quat_vmult(&lin_meas.vel, stateGetNedToBodyQuat_f(),
+ (struct FloatVect3 *)stateGetSpeedNed_f()); // From Kalman filter
+ lin_meas.acc = *stateGetAccelBody_f(); // From accelerometer
+
+ // Fetch attitude measurements
+ struct AttStateQuat attitude_meas;
+ attitude_meas.att = *stateGetNedToBodyQuat_f(); // From Kalman filter
+ attitude_meas.att_d = *stateGetBodyRates_f(); // From gyroscope
+ attitude_meas.att_2d.x = (attitude_meas.att_d.p - rates_prev.p) * PERIODIC_FREQUENCY;
+ attitude_meas.att_2d.y = (attitude_meas.att_d.q - rates_prev.q) * PERIODIC_FREQUENCY;
+ attitude_meas.att_2d.z = (attitude_meas.att_d.r - rates_prev.r) * PERIODIC_FREQUENCY;
+ rates_prev = attitude_meas.att_d; // Store previous rates for next acceleration calculation
+
+ // Propagate and get on board model actuator state estimates
+ float actuator_obm[ANDI_NUM_ACT];
+ update_first_order_zoh_low_pass_array(ANDI_NUM_ACT, actuator_obm_zohlpf, u_cmd);
+ get_first_order_zoh_low_pass_array(ANDI_NUM_ACT, actuator_obm_zohlpf, actuator_obm);
+ update_transport_delay_array(ANDI_NUM_ACT, actuator_obm_delay, actuator_obm);
+ get_transport_delay_array(ANDI_NUM_ACT, actuator_obm_delay, actuator_obm);
+
+ // Get actuator measurements from T4 feedback
+ fetch_actuators_t4(actuator_meas, &actuators_t4_obs);
+
+ // FIXME: Make configurable which source to use for each actuator.
+ // Choose actuator feedback for elevon and on board model for esc (because lack of rpm control).
+ actuator_state[0] = actuator_meas[0];
+ actuator_state[1] = actuator_meas[1];
+ actuator_state[2] = actuator_obm[2];
+ actuator_state[3] = actuator_obm[3];
+
+ // Evaluate On Board Model with previous filtered state estimates
+ struct FloatVect3 angular_accel_obm = evaluate_obm_moments(&attitude_state_cf.att_d, &linear_state_cf.vel,
+ actuator_state);
+ struct FloatVect3 linear_accel_obm = evaluate_obm_forces(&attitude_state_cf.att_d, &linear_state_cf.vel,
+ actuator_state);
+
+ // FIXME: Integral before filtering can cause numerical issues. Update complementary filter to handle this case.
+ // FIXME: Numerical differentiation to get accelerations can be noisy. Consider using a filter before differentiation.
+ // Cascaded complementary filter for linear velocity and accelerations measurements
+ update_butterworth_2_complementary_vect3(&linear_accel_cf, &linear_accel_obm, &lin_meas.acc);
+ linear_state_cf.acc = get_butterworth_2_complementary_vect3(&linear_accel_cf);
+ float_vect3_integrate_fi(&linear_velocity_obm, &linear_state_cf.acc, SAMPLE_TIME);
+ update_first_order_complementary_vect3(&linear_vel_cf, &linear_velocity_obm, &lin_meas.vel);
+ linear_state_cf.vel = get_first_order_complementary_vect3(&linear_vel_cf);
+
+ // Cascaded complementary filter for angular rates and accelerations measurements
+ update_butterworth_2_complementary_vect3(&attitude_accel_cf, &angular_accel_obm, &attitude_meas.att_2d);
+ attitude_state_cf.att_2d = get_butterworth_2_complementary_vect3(&attitude_accel_cf);
+ float_rates_vect3_integrate_fi(&angular_rates_obm, &attitude_state_cf.att_2d, SAMPLE_TIME);
+ update_first_order_complementary_rates(&attitude_rates_cf, &angular_rates_obm, &attitude_meas.att_d);
+ attitude_state_cf.att_d = get_first_order_complementary_rates(&attitude_rates_cf);
+ attitude_state_cf.att = attitude_meas.att;
+
+ // Get thrust measurement from on board model
+ thrust_state = evaluate_obm_thrust_z(actuator_state);
+
+ // Reconstructed nu is repurposed to hold modeled moments and specific thrust from the on board model (for obm validation).
+ // This can be used to compare the on board model prediction to the actual measured acceleration to validate model accuracy.
+ // Do not compare this to the complementary filter values since these are influenced by the model, use the raw measurements instead.
+ // nu_reconstructed[0] = angular_accel_obm.x;
+ // nu_reconstructed[1] = angular_accel_obm.y;
+ // nu_reconstructed[2] = angular_accel_obm.z;
+ // nu_reconstructed[3] = thrust_state;
+
+ // Get setpoints
+ if (use_rate_control) {
+ rates_des = stab_sp_to_rates_f(stab_setpoint);
+ if (in_flight)
+ generate_reference_rate(SAMPLE_TIME, &rates_des, &andi_k_rate_rm, &attitude_bounds, &attitude_ref);
+ } else {
+ attitude_des = stab_sp_to_quat_f(stab_setpoint);
+ if (in_flight)
+ generate_reference_attitude(SAMPLE_TIME, &attitude_des, &andi_k_att_rm, &attitude_bounds, &attitude_ref);
+ }
+
+ // FIXME: Thrust setpoint cant be of type THRUST_INCR_SP, this is not enforced but will silently fail
+ thrust_des = th_sp_to_thrust_f(thrust_setpoint, 0, THRUST_AXIS_Z) * THRUST_MAX;
+ generate_reference_thrust(SAMPLE_TIME, thrust_des, andi_k_thrust_rm, &thrust_bounds_min, &thrust_bounds_max,
+ &thrust_ref);
+
+ // Construct pseudo control
+ struct FloatVect3 nu_attitude;
+ if (use_rate_control) {
+ nu_attitude = control_error_rate(&attitude_ref, &attitude_state_cf, &andi_k_rate_ec);
+ } else {
+ nu_attitude = control_error_attitude(&attitude_ref, &attitude_state_cf, &andi_k_att_ec);
+ }
+ float nu_thrust = control_error_thrust(&thrust_ref, thrust_state, andi_k_thrust_ec);
+
+ nu_ec[0] = nu_attitude.x;
+ nu_ec[1] = nu_attitude.y;
+ nu_ec[2] = nu_attitude.z;
+ nu_ec[3] = nu_thrust;
+
+ // State feedback from on board model
+ if (USE_STATE_DYNAMICS) evaluate_obm_f_stb_x(nu_obm, &attitude_state_cf.att_d, &linear_state_cf.vel,
+ &attitude_state_cf.att_2d, &linear_state_cf.acc, actuator_state);
+ else float_vect_zero(nu_obm, ANDI_OUTPUTS);
+
+ if (in_flight) {
+ nu_obj[0] = nu_ec[0] - (nu_obm[0] * ANDI_RELAX_OBM);
+ nu_obj[1] = nu_ec[1] - (nu_obm[1] * ANDI_RELAX_OBM);
+ nu_obj[2] = nu_ec[2] - (nu_obm[2] * ANDI_RELAX_OBM);
+ } else {
+ nu_obj[0] = 0.0f;
+ nu_obj[1] = 0.0f;
+ nu_obj[2] = 0.0f;
+ }
+ nu_obj[3] = nu_ec[3] - (nu_obm[3] * ANDI_RELAX_OBM);
+
+ // Compute control effectiveness matrix based on current states
+ if (SCHEDULE_EFF) evaluate_obm_f_stb_u(ce_mat, &attitude_state_cf.att_d, &linear_state_cf.vel, actuator_state);
+
+ // Solve control allocation using weighted least squares
+ compute_wls_lower_bounds(du_min, actuator_state, ACTUATOR_MIN, ACTUATOR_D_MIN, ACTUATOR_DYNAMICS);
+ compute_wls_upper_bounds(du_max, actuator_state, ACTUATOR_MAX, ACTUATOR_D_MAX, ACTUATOR_DYNAMICS);
+ float wls_u_scaler[ANDI_NUM_ACT];
+ float wls_v_scaler[ANDI_OUTPUTS] = {[0 ... ANDI_OUTPUTS - 1] = 1.0f}; // Disable v scaling
+ compute_wls_u_scaler(wls_u_scaler, du_min,
+ du_max); // FIXME: Compute only once in advance for fixed bounds (important for well defined WLS Wu weights)
+ // compute_wls_v_scaler(wls_v_scaler, nu_obj);
+
+ float ce_mat_scaled[ANDI_OUTPUTS][ANDI_NUM_ACT];
+ float *bwls[ANDI_OUTPUTS];
+ // Scale control effectiveness matrix
+ for (uint8_t i = 0; i < ANDI_OUTPUTS; i++) {
+ for (uint8_t j = 0; j < ANDI_NUM_ACT; j++) {
+ ce_mat_scaled[i][j] = ce_mat[i * ANDI_NUM_ACT + j] * wls_v_scaler[i] / wls_u_scaler[j];
+ }
+ bwls[i] = ce_mat_scaled[i];
+ }
+
+ // Scale actuator bounds and weights
+ for (uint8_t i = 0; i < ANDI_NUM_ACT; i++) {
+ wls_stab_p.u_min[i] = du_min[i] * wls_u_scaler[i];
+ wls_stab_p.u_max[i] = du_max[i] * wls_u_scaler[i];
+ wls_stab_p.u_pref[i] = (wls_stab_p.u_min[i] + wls_stab_p.u_max[i]) / 2.0f; // Use mid-point as preferred command
+ wls_stab_p.Wu[i] = WLS_WU[i];
+ }
+
+ // Scale pseudo control and weights
+ for (uint8_t i = 0; i < ANDI_OUTPUTS; i++) {
+ wls_stab_p.v[i] = nu_obj[i] * wls_v_scaler[i];
+ wls_stab_p.Wv[i] = WLS_WV[i];
+ }
+
+ wls_alloc(&wls_stab_p, bwls, 0, 0, 10);
+
+ // Scale back actuator commands
+ for (uint8_t i = 0; i < ANDI_NUM_ACT; i++) {
+ du_cmd[i] = (wls_stab_p.u[i] / wls_u_scaler[i]);
+ u_cmd[i] = du_cmd[i] / ACTUATOR_DYNAMICS[i] + actuator_state[i];
+ Bound(u_cmd[i], ACTUATOR_MIN[i], ACTUATOR_MAX[i]);
+ }
+
+ // Commit actuator commands
+ // Resulting commands are in rad for servo and rad/s for motor
+ // Paparazzi expects the commands in pprz units (-MAX_PPRZ to MAX_PPRZ for servo, 0 to MAX_PPRZ for motor).
+ // FIXME: Do not hardcode actuator layout
+ commands[0] = (pprz_t)(u_cmd[0] / ACTUATOR_MAX[0] * MAX_PPRZ);
+ commands[1] = (pprz_t)(u_cmd[1] / ACTUATOR_MAX[1] * MAX_PPRZ);
+ commands[2] = (pprz_t)(sqrt(u_cmd[2] / ACTUATOR_MAX[2]) * MAX_PPRZ);
+ commands[3] = (pprz_t)(sqrt(u_cmd[3] / ACTUATOR_MAX[3]) * MAX_PPRZ);
+
+ // Set Thrust command for compatibility with other modules
+ // FIXME: Do not hardcode actuator layout
+ cmd[COMMAND_THRUST] = 0;
+ cmd[COMMAND_THRUST] += (pprz_t)(sqrt(u_cmd[2] / ACTUATOR_MAX[2]) * MAX_PPRZ);
+ cmd[COMMAND_THRUST] += (pprz_t)(sqrt(u_cmd[3] / ACTUATOR_MAX[3]) * MAX_PPRZ);
+ cmd[COMMAND_THRUST] /= 2;
+}
+
+// FIXME: The following functions are to integrate the controller in the existing stabilization framework, find a better way to do this or move.
+void stabilization_attitude_enter(void)
+{
+ ;
+}
+
+void stabilization_rate_enter(void)
+{
+ ;
+}
+
+void stabilization_rate_run(bool in_flight __attribute__((unused)),
+ struct StabilizationSetpoint *rate_sp __attribute__((unused)), struct ThrustSetpoint *thrust __attribute__((unused)),
+ int32_t *cmd __attribute__((unused)))
+{
+ ;
+}
+
+void stabilization_attitude_run(bool in_flight __attribute__((unused)),
+ struct StabilizationSetpoint *sp __attribute__((unused)), struct ThrustSetpoint *thrust __attribute__((unused)),
+ int32_t *cmd __attribute__((unused)))
+{
+ ;
+}
+
+// FIXME: This function is duplicated in stabilization_rate.c, find a way to share it
+struct StabilizationSetpoint stabilization_rate_read_rc(struct RadioControl *rc)
+{
+ struct FloatRates rate_sp;
+ FLOAT_RATES_ZERO(rate_sp);
+ if (ROLL_RATE_DEADBAND_EXCEEDED(rc)) {
+ rate_sp.p = rc->values[RC_RATE_P] * RC_RATE_MAX[0] / MAX_PPRZ;
+ }
+ if (PITCH_RATE_DEADBAND_EXCEEDED(rc)) {
+ rate_sp.q = rc->values[RC_RATE_Q] * RC_RATE_MAX[1] / MAX_PPRZ;
+ }
+ if (YAW_RATE_DEADBAND_EXCEEDED(rc)) {
+ rate_sp.r = rc->values[RC_RATE_R] * RC_RATE_MAX[2] / MAX_PPRZ;
+ }
+ return stab_sp_from_rates_f(&rate_sp);
+}
+
+// FIXME: Autopilot end flight detection is not applicable to tailsitters.
+bool autopilot_in_flight_end_detection(bool motors_on __attribute__((unused)))
+{
+ return false;
+}
+
+// DEBUG FUNCTION
+// static int counter = 0;
+// /**
+// * @brief Debug function for actuators.
+// *
+// * Send step inputs to actuators and observe responses.
+// * useful to estimate actuator dynamics.
+// */
+// void actuator_debug(bool do_servo_step, bool do_motor_step, float step_rate)
+// {
+// if (counter++ > PERIODIC_FREQUENCY * step_rate) {
+// counter = 0;
+// }
+
+// if (counter <= (PERIODIC_FREQUENCY * step_rate) / 2) {
+// commands[0] = (pprz_t)(do_servo_step ? MAX_PPRZ : 0);
+// commands[1] = (pprz_t)(do_servo_step ? MAX_PPRZ : 0);
+// commands[2] = (pprz_t)(do_motor_step ? MAX_PPRZ : 0);
+// commands[3] = (pprz_t)(do_motor_step ? MAX_PPRZ : 0);
+// } else {
+// commands[0] = (pprz_t)(do_servo_step ? MIN_PPRZ : 0);
+// commands[1] = (pprz_t)(do_servo_step ? MIN_PPRZ : 0);
+// commands[2] = (pprz_t)(do_motor_step ? 0 : 0);
+// commands[3] = (pprz_t)(do_motor_step ? 0 : 0);
+// }
+// }
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_andi.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_andi.h
new file mode 100644
index 0000000000..04f44d8f11
--- /dev/null
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_andi.h
@@ -0,0 +1,277 @@
+/*
+ *
+ * Copyright (C) 2025 Justin Dubois
+ *
+ * This file is part of paparazzi
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ *
+ */
+
+/**
+ * @file sw/airborne/firmwares/rotorcraft/stabilization/stabilization_andi.h
+ * @brief ANDI stabilization controller for tiltbody rotorcraft.
+ *
+ * Provides declarations, configuration parameters, and interfaces for the ANDI controller implemented in stabilization_andi.c.
+ *
+ * @see stabilization_andi.c
+ * @author Justin Dubois
+ */
+
+
+#ifndef STABILIZATION_ANDI_H
+#define STABILIZATION_ANDI_H
+
+#include "firmwares/rotorcraft/stabilization.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_rate.h"
+#include "generated/airframe.h"
+#include
+#include "filters/low_pass_filter.h"
+
+
+#ifndef ANDI_NUM_ACT
+ #define ANDI_NUM_ACT COMMANDS_NB_REAL
+#endif
+
+#ifndef ANDI_OUTPUTS
+ #error "You must specify the number of controlled axis (outputs)"
+#endif
+
+/**
+ * @brief Structure representing attitude in quaternion form along with its first three derivatives, used for reference.
+ *
+ * This structure encapsulates the attitude represented as a quaternion,
+ * its first derivative (angular rates), second derivative (angular accelerations),
+ * and third derivative (angular jerks). It is primarily used for reference
+ * representation in the ANDI controller.
+ */
+struct AttQuat {
+ struct FloatQuat att;
+ struct FloatRates att_d;
+ struct FloatVect3 att_2d;
+ struct FloatVect3 att_3d;
+};
+
+/**
+ * @brief Structure representing attitude in quaternion form along with its first two derivatives, used for state.
+ *
+ * This structure encapsulates the attitude represented as a quaternion,
+ * its first derivative (angular rates), and second derivative (angular accelerations).
+ * It is primarily used for state representation in the ANDI controller.
+ */
+struct AttStateQuat {
+ struct FloatQuat att;
+ struct FloatRates att_d;
+ struct FloatVect3 att_2d;
+};
+
+/**
+ * @brief Structure representing linear state with velocity and acceleration vectors.
+ *
+ * This structure encapsulates the linear state of the vehicle,
+ * including its velocity and acceleration as 3D vectors.
+ */
+struct LinState {
+ struct FloatVect3 vel;
+ struct FloatVect3 acc;
+};
+
+/**
+ * @brief Structure representing (specific) thrust reference and its derivative.
+ *
+ * This structure encapsulates the thrust reference,
+ * including the thrust value and its first derivative.
+ */
+struct ThrustRef {
+ float thrust;
+ float thrust_d;
+};
+
+/**
+ * @brief Structure defining third-order pole parameters for vectorized 3D quantities.
+ *
+ * This structure holds the natural frequencies, damping ratios,
+ * and the pseudo actuator dynamics.
+ */
+struct PolesOrder3Vect3 {
+ struct FloatVect3 omega_n;
+ struct FloatVect3 zeta;
+ struct FloatVect3 omega_a;
+};
+
+/**
+ * @brief Structure defining second-order pole parameters for vectorized 3D quantities.
+ *
+ * This structure holds the natural frequencies and damping ratios.
+ */
+struct PolesOrder2Vect3 {
+ struct FloatVect3 omega_a;
+ struct FloatVect3 zeta;
+};
+
+/**
+ * @brief Structure defining second-order gain parameters for vectorized 3D quantities.
+ */
+struct GainsOrder2Vect3 {
+ struct FloatVect3 k1;
+ struct FloatVect3 k2;
+};
+
+/**
+ * @brief Structure defining third-order gain parameters for vectorized 3D quantities.
+ */
+struct GainsOrder3Vect3 {
+ struct FloatVect3 k1;
+ struct FloatVect3 k2;
+ struct FloatVect3 k3;
+};
+
+/* Declaration of Reference Model and Error Controller Poles */
+extern struct PolesOrder2Vect3 andi_p_rate_ec;
+extern struct PolesOrder2Vect3 andi_p_rate_rm;
+extern struct PolesOrder3Vect3 andi_p_att_ec;
+extern struct PolesOrder3Vect3 andi_p_att_rm;
+extern float andi_p_thrust_ec;
+extern float andi_p_thrust_rm;
+
+void stabilization_andi_init(void);
+void stabilization_andi_enter(void);
+void stabilization_andi_run(bool use_rate_control, bool in_flight, struct StabilizationSetpoint *stab_setpoint,
+ struct ThrustSetpoint *thrust_setpoint, int32_t *cmd);
+// void actuator_debug(bool do_servo_step, bool do_motor_step, float step_rate);
+
+/**
+ * @brief Evaluate total force acting on the vehicle from the OBM
+ *
+ * Computes the full set of forces modeled by the on board model for
+ * the given airframe as a function of the state and input.
+ *
+ * This function is used for complementary filtering in ANDI.
+ *
+ * @param[in] rates Current angular rates (p, q, r) used by the model (rad/s).
+ * @param[in] vel_body Body-frame linear velocity vector (u, v, w) (m/s).
+ * @param[in] actuator_state Current actuator commands/deflections array (length
+ * ANDI_NUM_ACT). Units and normalization depend on the
+ * airframe and actuator type (e.g. radians, throttle fraction).
+ * @return Computed force vector produced by the actuators as per the OBM.
+ *
+ * @note Implementation is airframe-specific.
+ * @see obm_cyclone.c
+ */
+struct FloatVect3 evaluate_obm_forces(const struct FloatRates *rates, const struct FloatVect3 *vel_body,
+ const float actuator_state[ANDI_NUM_ACT]);
+
+/**
+ * @brief Evaluate total moments acting on the vehicle from the OBM
+ *
+ * Computes the full set of moments modeled by the on board model for
+ * the given airframe as a function of the state and input.
+ *
+ * This function is used for complementary filtering in ANDI.
+ *
+ * @param[in] rates Current angular rates (p, q, r) used by the model (rad/s).
+ * @param[in] vel_body Body-frame linear velocity vector (u, v, w) (m/s).
+ * @param[in] actuator_state Current actuator commands/deflections array (length
+ * ANDI_NUM_ACT). Units and normalization depend on the
+ * airframe and actuator type (e.g. radians, throttle fraction).
+ * @return Computed moment vector produced by the actuators as per the OBM.
+ *
+ * @note Implementation is airframe-specific.
+ * @see obm_cyclone.c
+ */
+struct FloatVect3 evaluate_obm_moments(const struct FloatRates *rates, const struct FloatVect3 *vel_body,
+ const float actuator_state[ANDI_NUM_ACT]);
+
+/**
+ * @brief Evaluate the state-dependent control effectiveness matrix F_u for stabilization.
+ *
+ * This function computes the mapping from actuator inputs to aerodynamic/motor outputs
+ * for the stabilization model. The output is written into the provided, pre-allocated
+ * array fu_mat of size ANDI_NUM_ACT * ANDI_OUTPUTS.
+ *
+ * The produced matrix represents the partial derivatives of the modeled outputs with
+ * respect to actuator commands (∂f/∂u) evaluated at the current state and actuator
+ * conditions. It is intended to be implemented per-airframe (see obm_cyclone for an
+ * example).
+ *
+ * @param[out] fu_mat Pre-allocated array (size ANDI_NUM_ACT * ANDI_OUTPUTS) receiving
+ * the flattened control-effectiveness matrix. Convention: element
+ * for output i and actuator j should be written to
+ * fu_mat[i * ANDI_NUM_ACT + j].
+ * @param[in] rates Current angular rates (typically p, q, r) used by the model (rad/s).
+ * @param[in] vel_body Body-frame linear velocity vector (u, v, w) (m/s).
+ * @param[in] actuator_state Current actuator commands/deflections array (length
+ * ANDI_NUM_ACT). Units and normalization depend on the
+ * airframe and actuator type (e.g. radians, throttle fraction).
+ *
+ * @note Implementations are airframe-specific and must account for the particular
+ * actuator layout and aerodynamic/motor characteristics of the platform.
+ * @see obm_cyclone.c
+ */
+void evaluate_obm_f_stb_u(float fu_mat[ANDI_NUM_ACT * ANDI_OUTPUTS], const struct FloatRates *rates,
+ const struct FloatVect3 *vel_body, const float actuator_state[ANDI_NUM_ACT]);
+
+/**
+ * @brief Evaluate the state-dependent contribution F_x * x_dot for stabilization.
+ *
+ * Computes nu_obm = F_x * x_dot, the portion of the open-body-model outputs that
+ * depends on the current state rates/accelerations (i.e. the state-dependent term).
+ * This is the term that captures how changes in state drive the modeled outputs
+ * independent of actuator inputs.
+ *
+ * @note This contribution is often neglected for INDI, but can be included for ANDI.
+ *
+ * @param[out] nu_obm Pre-allocated output vector of length ANDI_OUTPUTS receiving
+ * the computed F_x * x_dot values.
+ * @param[in] rates Current angular rates (p, q, r) used by the model (rad/s).
+ * @param[in] vel_body Body-frame velocity vector (u, v, w) (m/s).
+ * @param[in] ang_accel Body angular accelerations (p_dot, q_dot, r_dot) (rad/s^2).
+ * @param[in] accel_body Body-frame linear accelerations (ax, ay, az) (m/s^2).
+ * @param[in] actuator_state Current actuator commands/deflections array (length
+ * ANDI_NUM_ACT). Some terms of F_x may be actuator-state
+ * dependent (e.g. rotor wake effects).
+ *
+ * @note The function computes only the state-dependent part of the model. The full
+ * modeled output is typically nu_obm + F_u * delta_u (where F_u is provided by
+ * evaluate_obm_f_stb_u). Implementations must be provided per airframe.
+ * @see obm_cyclone.c
+ */
+void evaluate_obm_f_stb_x(float nu_obm[ANDI_OUTPUTS], const struct FloatRates *rates, const struct FloatVect3 *vel_body,
+ const struct FloatVect3 *ang_accel, const struct FloatVect3 *accel_body, const float actuator_state[ANDI_NUM_ACT]);
+
+
+/**
+ * @brief Compute total thrust produced by the current actuator state.
+ *
+ * Returns the aggregate thrust generated by the set of actuators described by
+ * actuator_state. This value is typically used by the stabilization/obm code when
+ * converting actuator commands to net force for direct thrust control.
+ *
+ * @note This function does not intend to accurately compute the thrust force;
+ * it only returns an approximation of the total specific thrust in the body z-direction.
+ * The intended use is as feedback for direct throttle control in ANDI stabilization.
+ *
+ * @param[in] actuator_state Current actuator commands/deflections array (length
+ * ANDI_NUM_ACT). Units and normalization depend on the
+ * airframe (e.g. rotor collective, throttle fraction).
+ * @return Total specific thrust produced by the actuators (SI units, m/s^2). If
+ * the airframe model uses a normalized thrust unit, document that convention
+ * in the airframe implementation.
+ *
+ * @note Implementation is airframe-specific.
+ * @see obm_cyclone.c
+ */
+float evaluate_obm_thrust_z(const float actuator_state[ANDI_NUM_ACT]);
+#endif // STABILIZATION_ANDI_H
\ No newline at end of file
diff --git a/sw/airborne/math/pprz_algebra_float.c b/sw/airborne/math/pprz_algebra_float.c
index 09300d066a..c14e3df65b 100644
--- a/sw/airborne/math/pprz_algebra_float.c
+++ b/sw/airborne/math/pprz_algebra_float.c
@@ -27,7 +27,7 @@
#include "pprz_algebra_float.h"
/** in place first order integration of a 3D-vector */
-void float_vect3_integrate_fi(struct FloatVect3 *vec, struct FloatVect3 *dv, float dt)
+void float_vect3_integrate_fi(struct FloatVect3 *vec, const struct FloatVect3 *dv, const float dt)
{
vec->x += dv->x * dt;
vec->y += dv->y * dt;
@@ -35,14 +35,22 @@ void float_vect3_integrate_fi(struct FloatVect3 *vec, struct FloatVect3 *dv, flo
}
/** in place first order integration of angular rates */
-void float_rates_integrate_fi(struct FloatRates *r, struct FloatRates *dr, float dt)
+void float_rates_integrate_fi(struct FloatRates *r, const struct FloatRates *dr, const float dt)
{
r->p += dr->p * dt;
r->q += dr->q * dt;
r->r += dr->r * dt;
}
-void float_rates_of_euler_dot(struct FloatRates *r, struct FloatEulers *e, struct FloatEulers *edot)
+/** in place first order integration of angular rates */
+void float_rates_vect3_integrate_fi(struct FloatRates *r, const struct FloatVect3 *dr, const float dt)
+{
+ r->p += dr->x * dt;
+ r->q += dr->y * dt;
+ r->r += dr->z * dt;
+}
+
+void float_rates_of_euler_dot(struct FloatRates *r, const struct FloatEulers *e, const struct FloatEulers *edot)
{
r->p = edot->phi - sinf(e->theta) * edot->psi;
r->q = cosf(e->phi) * edot->theta + sinf(e->phi) * cosf(e->theta) * edot->psi;
@@ -52,7 +60,7 @@ void float_rates_of_euler_dot(struct FloatRates *r, struct FloatEulers *e, struc
-void float_rmat_inv(struct FloatRMat *m_b2a, struct FloatRMat *m_a2b)
+void float_rmat_inv(struct FloatRMat *m_b2a, const struct FloatRMat *m_a2b)
{
RMAT_ELMT(*m_b2a, 0, 0) = RMAT_ELMT(*m_a2b, 0, 0);
RMAT_ELMT(*m_b2a, 0, 1) = RMAT_ELMT(*m_a2b, 1, 0);
@@ -65,7 +73,7 @@ void float_rmat_inv(struct FloatRMat *m_b2a, struct FloatRMat *m_a2b)
RMAT_ELMT(*m_b2a, 2, 2) = RMAT_ELMT(*m_a2b, 2, 2);
}
-float float_rmat_norm(struct FloatRMat *rm)
+float float_rmat_norm(const struct FloatRMat *rm)
{
return sqrtf(SQUARE(rm->m[0]) + SQUARE(rm->m[1]) + SQUARE(rm->m[2]) +
SQUARE(rm->m[3]) + SQUARE(rm->m[4]) + SQUARE(rm->m[5]) +
@@ -75,7 +83,7 @@ float float_rmat_norm(struct FloatRMat *rm)
/** Composition (multiplication) of two rotation matrices.
* m_a2c = m_a2b comp m_b2c , aka m_a2c = m_b2c * m_a2b
*/
-void float_rmat_comp(struct FloatRMat *m_a2c, struct FloatRMat *m_a2b, struct FloatRMat *m_b2c)
+void float_rmat_comp(struct FloatRMat *m_a2c, const struct FloatRMat *m_a2b, const struct FloatRMat *m_b2c)
{
m_a2c->m[0] = m_b2c->m[0] * m_a2b->m[0] + m_b2c->m[1] * m_a2b->m[3] + m_b2c->m[2] * m_a2b->m[6];
m_a2c->m[1] = m_b2c->m[0] * m_a2b->m[1] + m_b2c->m[1] * m_a2b->m[4] + m_b2c->m[2] * m_a2b->m[7];
@@ -91,7 +99,7 @@ void float_rmat_comp(struct FloatRMat *m_a2c, struct FloatRMat *m_a2b, struct Fl
/** Composition (multiplication) of two rotation matrices.
* m_a2b = m_a2c comp_inv m_b2c , aka m_a2b = inv(_m_b2c) * m_a2c
*/
-void float_rmat_comp_inv(struct FloatRMat *m_a2b, struct FloatRMat *m_a2c, struct FloatRMat *m_b2c)
+void float_rmat_comp_inv(struct FloatRMat *m_a2b, const struct FloatRMat *m_a2c, const struct FloatRMat *m_b2c)
{
m_a2b->m[0] = m_b2c->m[0] * m_a2c->m[0] + m_b2c->m[3] * m_a2c->m[3] + m_b2c->m[6] * m_a2c->m[6];
m_a2b->m[1] = m_b2c->m[0] * m_a2c->m[1] + m_b2c->m[3] * m_a2c->m[4] + m_b2c->m[6] * m_a2c->m[7];
@@ -107,7 +115,7 @@ void float_rmat_comp_inv(struct FloatRMat *m_a2b, struct FloatRMat *m_a2c, struc
/** rotate 3D vector by rotation matrix.
* vb = m_a2b * va
*/
-void float_rmat_vmult(struct FloatVect3 *vb, struct FloatRMat *m_a2b, struct FloatVect3 *va)
+void float_rmat_vmult(struct FloatVect3 *vb, const struct FloatRMat *m_a2b, const struct FloatVect3 *va)
{
vb->x = m_a2b->m[0] * va->x + m_a2b->m[1] * va->y + m_a2b->m[2] * va->z;
vb->y = m_a2b->m[3] * va->x + m_a2b->m[4] * va->y + m_a2b->m[5] * va->z;
@@ -117,7 +125,7 @@ void float_rmat_vmult(struct FloatVect3 *vb, struct FloatRMat *m_a2b, struct Flo
/** rotate 3D vector by transposed rotation matrix.
* vb = m_b2a^T * va
*/
-void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
+void float_rmat_transp_vmult(struct FloatVect3 *vb, const struct FloatRMat *m_b2a, const struct FloatVect3 *va)
{
vb->x = m_b2a->m[0] * va->x + m_b2a->m[3] * va->y + m_b2a->m[6] * va->z;
vb->y = m_b2a->m[1] * va->x + m_b2a->m[4] * va->y + m_b2a->m[7] * va->z;
@@ -127,7 +135,7 @@ void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, str
/** rotate angle by rotation matrix.
* rb = m_a2b * ra
*/
-void float_rmat_mult(struct FloatEulers *rb, struct FloatRMat *m_a2b, struct FloatEulers *ra)
+void float_rmat_mult(struct FloatEulers *rb, const struct FloatRMat *m_a2b, const struct FloatEulers *ra)
{
rb->phi = m_a2b->m[0] * ra->phi + m_a2b->m[1] * ra->theta + m_a2b->m[2] * ra->psi;
rb->theta = m_a2b->m[3] * ra->phi + m_a2b->m[4] * ra->theta + m_a2b->m[5] * ra->psi;
@@ -137,7 +145,7 @@ void float_rmat_mult(struct FloatEulers *rb, struct FloatRMat *m_a2b, struct Flo
/** rotate angle by transposed rotation matrix.
* rb = m_b2a^T * ra
*/
-void float_rmat_transp_mult(struct FloatEulers *rb, struct FloatRMat *m_b2a, struct FloatEulers *ra)
+void float_rmat_transp_mult(struct FloatEulers *rb, const struct FloatRMat *m_b2a, const struct FloatEulers *ra)
{
rb->phi = m_b2a->m[0] * ra->phi + m_b2a->m[3] * ra->theta + m_b2a->m[6] * ra->psi;
rb->theta = m_b2a->m[1] * ra->phi + m_b2a->m[4] * ra->theta + m_b2a->m[7] * ra->psi;
@@ -147,7 +155,7 @@ void float_rmat_transp_mult(struct FloatEulers *rb, struct FloatRMat *m_b2a, str
/** rotate anglular rates by rotation matrix.
* rb = m_a2b * ra
*/
-void float_rmat_ratemult(struct FloatRates *rb, struct FloatRMat *m_a2b, struct FloatRates *ra)
+void float_rmat_ratemult(struct FloatRates *rb, const struct FloatRMat *m_a2b, const struct FloatRates *ra)
{
rb->p = m_a2b->m[0] * ra->p + m_a2b->m[1] * ra->q + m_a2b->m[2] * ra->r;
rb->q = m_a2b->m[3] * ra->p + m_a2b->m[4] * ra->q + m_a2b->m[5] * ra->r;
@@ -157,7 +165,7 @@ void float_rmat_ratemult(struct FloatRates *rb, struct FloatRMat *m_a2b, struct
/** rotate anglular rates by transposed rotation matrix.
* rb = m_b2a^T * ra
*/
-void float_rmat_transp_ratemult(struct FloatRates *rb, struct FloatRMat *m_b2a, struct FloatRates *ra)
+void float_rmat_transp_ratemult(struct FloatRates *rb, const struct FloatRMat *m_b2a, const struct FloatRates *ra)
{
rb->p = m_b2a->m[0] * ra->p + m_b2a->m[3] * ra->q + m_b2a->m[6] * ra->r;
rb->q = m_b2a->m[1] * ra->p + m_b2a->m[4] * ra->q + m_b2a->m[7] * ra->r;
@@ -166,7 +174,7 @@ void float_rmat_transp_ratemult(struct FloatRates *rb, struct FloatRMat *m_b2a,
/** initialises a rotation matrix from unit vector axis and angle */
-void float_rmat_of_axis_angle(struct FloatRMat *rm, struct FloatVect3 *uv, float angle)
+void float_rmat_of_axis_angle(struct FloatRMat *rm, const struct FloatVect3 *uv, const float angle)
{
const float ux2 = uv->x * uv->x;
const float uy2 = uv->y * uv->y;
@@ -191,7 +199,7 @@ void float_rmat_of_axis_angle(struct FloatRMat *rm, struct FloatVect3 *uv, float
/* C n->b rotation matrix */
-void float_rmat_of_eulers_321(struct FloatRMat *rm, struct FloatEulers *e)
+void float_rmat_of_eulers_321(struct FloatRMat *rm, const struct FloatEulers *e)
{
const float sphi = sinf(e->phi);
const float cphi = cosf(e->phi);
@@ -211,7 +219,7 @@ void float_rmat_of_eulers_321(struct FloatRMat *rm, struct FloatEulers *e)
RMAT_ELMT(*rm, 2, 2) = cphi * ctheta;
}
-void float_rmat_of_eulers_312(struct FloatRMat *rm, struct FloatEulers *e)
+void float_rmat_of_eulers_312(struct FloatRMat *rm, const struct FloatEulers *e)
{
const float sphi = sinf(e->phi);
const float cphi = cosf(e->phi);
@@ -233,7 +241,7 @@ void float_rmat_of_eulers_312(struct FloatRMat *rm, struct FloatEulers *e)
/* C n->b rotation matrix */
-void float_rmat_of_quat(struct FloatRMat *rm, struct FloatQuat *q)
+void float_rmat_of_quat(struct FloatRMat *rm, const struct FloatQuat *q)
{
const float _a = M_SQRT2 * q->qi;
const float _b = M_SQRT2 * q->qx;
@@ -258,12 +266,12 @@ void float_rmat_of_quat(struct FloatRMat *rm, struct FloatQuat *q)
}
/** in place first order integration of a rotation matrix */
-void float_rmat_integrate_fi(struct FloatRMat *rm, struct FloatRates *omega, float dt)
+void float_rmat_integrate_fi(struct FloatRMat *rm, const struct FloatRates *omega, float dt)
{
struct FloatRMat exp_omega_dt = {
{
- 1. , dt *omega->r, -dt *omega->q,
- -dt *omega->r, 1. , dt *omega->p,
+ 1., dt *omega->r, -dt *omega->q,
+ -dt *omega->r, 1., dt *omega->p,
dt *omega->q, -dt *omega->p, 1.
}
};
@@ -286,12 +294,12 @@ static inline float renorm_factor(float n)
float float_rmat_reorthogonalize(struct FloatRMat *rm)
{
const struct FloatVect3 r0 = {RMAT_ELMT(*rm, 0, 0),
- RMAT_ELMT(*rm, 0, 1),
- RMAT_ELMT(*rm, 0, 2)
+ RMAT_ELMT(*rm, 0, 1),
+ RMAT_ELMT(*rm, 0, 2)
};
const struct FloatVect3 r1 = {RMAT_ELMT(*rm, 1, 0),
- RMAT_ELMT(*rm, 1, 1),
- RMAT_ELMT(*rm, 1, 2)
+ RMAT_ELMT(*rm, 1, 1),
+ RMAT_ELMT(*rm, 1, 2)
};
float _err = -0.5 * VECT3_DOT_PRODUCT(r0, r1);
struct FloatVect3 r0_t;
@@ -317,7 +325,7 @@ float float_rmat_reorthogonalize(struct FloatRMat *rm)
*
*/
-void float_quat_comp(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
+void float_quat_comp(struct FloatQuat *a2c, const struct FloatQuat *a2b, const struct FloatQuat *b2c)
{
a2c->qi = a2b->qi * b2c->qi - a2b->qx * b2c->qx - a2b->qy * b2c->qy - a2b->qz * b2c->qz;
a2c->qx = a2b->qi * b2c->qx + a2b->qx * b2c->qi + a2b->qy * b2c->qz - a2b->qz * b2c->qy;
@@ -325,7 +333,7 @@ void float_quat_comp(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQ
a2c->qz = a2b->qi * b2c->qz + a2b->qx * b2c->qy - a2b->qy * b2c->qx + a2b->qz * b2c->qi;
}
-void float_quat_comp_inv(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c)
+void float_quat_comp_inv(struct FloatQuat *a2b, const struct FloatQuat *a2c, const struct FloatQuat *b2c)
{
a2b->qi = a2c->qi * b2c->qi + a2c->qx * b2c->qx + a2c->qy * b2c->qy + a2c->qz * b2c->qz;
a2b->qx = -a2c->qi * b2c->qx + a2c->qx * b2c->qi - a2c->qy * b2c->qz + a2c->qz * b2c->qy;
@@ -333,7 +341,7 @@ void float_quat_comp_inv(struct FloatQuat *a2b, struct FloatQuat *a2c, struct Fl
a2b->qz = -a2c->qi * b2c->qz - a2c->qx * b2c->qy + a2c->qy * b2c->qx + a2c->qz * b2c->qi;
}
-void float_quat_inv_comp(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c)
+void float_quat_inv_comp(struct FloatQuat *b2c, const struct FloatQuat *a2b, const struct FloatQuat *a2c)
{
b2c->qi = a2b->qi * a2c->qi + a2b->qx * a2c->qx + a2b->qy * a2c->qy + a2b->qz * a2c->qz;
b2c->qx = a2b->qi * a2c->qx - a2b->qx * a2c->qi - a2b->qy * a2c->qz + a2b->qz * a2c->qy;
@@ -341,28 +349,28 @@ void float_quat_inv_comp(struct FloatQuat *b2c, struct FloatQuat *a2b, struct Fl
b2c->qz = a2b->qi * a2c->qz - a2b->qx * a2c->qy + a2b->qy * a2c->qx - a2b->qz * a2c->qi;
}
-void float_quat_comp_norm_shortest(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
+void float_quat_comp_norm_shortest(struct FloatQuat *a2c, const struct FloatQuat *a2b, const struct FloatQuat *b2c)
{
float_quat_comp(a2c, a2b, b2c);
float_quat_wrap_shortest(a2c);
float_quat_normalize(a2c);
}
-void float_quat_comp_inv_norm_shortest(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c)
+void float_quat_comp_inv_norm_shortest(struct FloatQuat *a2b, const struct FloatQuat *a2c, const struct FloatQuat *b2c)
{
float_quat_comp_inv(a2b, a2c, b2c);
float_quat_wrap_shortest(a2b);
float_quat_normalize(a2b);
}
-void float_quat_inv_comp_norm_shortest(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c)
+void float_quat_inv_comp_norm_shortest(struct FloatQuat *b2c, const struct FloatQuat *a2b, const struct FloatQuat *a2c)
{
float_quat_inv_comp(b2c, a2b, a2c);
float_quat_wrap_shortest(b2c);
float_quat_normalize(b2c);
}
-void float_quat_differential(struct FloatQuat *q_out, struct FloatRates *w, float dt)
+void float_quat_differential(struct FloatQuat *q_out, const struct FloatRates *w, const float dt)
{
const float v_norm = sqrtf(w->p * w->p + w->q * w->q + w->r * w->r);
const float c2 = cos(dt * v_norm / 2.0);
@@ -381,7 +389,7 @@ void float_quat_differential(struct FloatQuat *q_out, struct FloatRates *w, floa
}
/** in place first order quaternion integration with constant rotational velocity */
-void float_quat_integrate_fi(struct FloatQuat *q, struct FloatRates *omega, float dt)
+void float_quat_integrate_fi(struct FloatQuat *q, const struct FloatRates *omega, const float dt)
{
const float qi = q->qi;
const float qx = q->qx;
@@ -397,7 +405,7 @@ void float_quat_integrate_fi(struct FloatQuat *q, struct FloatRates *omega, floa
}
/** in place quaternion integration with constant rotational velocity */
-void float_quat_integrate(struct FloatQuat *q, struct FloatRates *omega, float dt)
+void float_quat_integrate(struct FloatQuat *q, const struct FloatRates *omega, const float dt)
{
const float no = FLOAT_RATES_NORM(*omega);
if (no > FLT_MIN) {
@@ -418,7 +426,7 @@ void float_quat_integrate(struct FloatQuat *q, struct FloatRates *omega, float d
}
}
-void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q, const struct FloatVect3 *v_in)
+void float_quat_vmult(struct FloatVect3 *v_out, const struct FloatQuat *q, const struct FloatVect3 *v_in)
{
const float qi2_M1_2 = q->qi * q->qi - 0.5;
const float qiqx = q->qi * q->qx;
@@ -447,7 +455,7 @@ void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q, const struc
* or equally:
* qd = 0.5 * q * omega(r)
*/
-void float_quat_derivative(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q)
+void float_quat_derivative(struct FloatQuat *qd, const struct FloatRates *r, const struct FloatQuat *q)
{
qd->qi = -0.5 * (r->p * q->qx + r->q * q->qy + r->r * q->qz);
qd->qx = -0.5 * (-r->p * q->qi - r->r * q->qy + r->q * q->qz);
@@ -458,7 +466,7 @@ void float_quat_derivative(struct FloatQuat *qd, struct FloatRates *r, struct Fl
/** Quaternion derivative from rotational velocity.
* qd = -0.5*omega(r) * q
*/
-void float_quat_derivative_lagrange(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q)
+void float_quat_derivative_lagrange(struct FloatQuat *qd, const struct FloatRates *r, const struct FloatQuat *q)
{
const float K_LAGRANGE = 1.;
const float c = K_LAGRANGE * (1 - float_quat_norm(q)) / -0.5;
@@ -474,7 +482,7 @@ void float_quat_derivative_lagrange(struct FloatQuat *qd, struct FloatRates *r,
* @param q Quat output
* @param e Euler input
*/
-void float_quat_of_eulers(struct FloatQuat *q, struct FloatEulers *e)
+void float_quat_of_eulers(struct FloatQuat *q, const struct FloatEulers *e)
{
const float phi2 = e->phi / 2.f;
@@ -501,7 +509,7 @@ void float_quat_of_eulers(struct FloatQuat *q, struct FloatEulers *e)
* @param q Quat output
* @param e Euler input
*/
-void float_quat_of_eulers_zxy(struct FloatQuat *q, struct FloatEulers *e)
+void float_quat_of_eulers_zxy(struct FloatQuat *q, const struct FloatEulers *e)
{
const float phi2 = e->phi / 2.f;
const float theta2 = e->theta / 2.f;
@@ -529,7 +537,7 @@ void float_quat_of_eulers_zxy(struct FloatQuat *q, struct FloatEulers *e)
* @param q Quat output
* @param e Euler input
*/
-void float_quat_of_eulers_yxz(struct FloatQuat *q, struct FloatEulers *e)
+void float_quat_of_eulers_yxz(struct FloatQuat *q, const struct FloatEulers *e)
{
const float phi2 = e->phi / 2.f;
const float theta2 = e->theta / 2.f;
@@ -548,7 +556,7 @@ void float_quat_of_eulers_yxz(struct FloatQuat *q, struct FloatEulers *e)
q->qz = c_theta2 * c_phi2 * s_psi2 - s_theta2 * s_phi2 * c_psi2;
}
-void float_quat_of_axis_angle(struct FloatQuat *q, const struct FloatVect3 *uv, float angle)
+void float_quat_of_axis_angle(struct FloatQuat *q, const struct FloatVect3 *uv, const float angle)
{
const float san = sinf(angle / 2.f);
q->qi = cosf(angle / 2.f);
@@ -574,7 +582,7 @@ void float_quat_of_orientation_vect(struct FloatQuat *q, const struct FloatVect3
}
}
-void float_quat_of_rmat(struct FloatQuat *q, struct FloatRMat *rm)
+void float_quat_of_rmat(struct FloatQuat *q, const struct FloatRMat *rm)
{
const float tr = RMAT_TRACE(*rm);
if (tr > 0) {
@@ -631,7 +639,7 @@ void float_quat_of_rmat(struct FloatQuat *q, struct FloatRMat *rm)
* @param twist Twist output
* @param quat Quaternion input
*/
-void float_quat_tilt_twist(struct FloatQuat *tilt, struct FloatQuat *twist, struct FloatQuat *quat)
+void float_quat_tilt_twist(struct FloatQuat *tilt, struct FloatQuat *twist, const struct FloatQuat *quat)
{
struct FloatVect3 z = {0., 0., 1.};
struct FloatVect3 z_rot;
@@ -663,7 +671,7 @@ void float_quat_tilt_twist(struct FloatQuat *tilt, struct FloatQuat *twist, stru
*
*/
-void float_eulers_of_rmat(struct FloatEulers *e, struct FloatRMat *rm)
+void float_eulers_of_rmat(struct FloatEulers *e, const struct FloatRMat *rm)
{
const float dcm00 = rm->m[0];
const float dcm01 = rm->m[1];
@@ -685,7 +693,7 @@ void float_eulers_of_rmat(struct FloatEulers *e, struct FloatRMat *rm)
* @param e Euler output
* @param q Quat input
*/
-void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
+void float_eulers_of_quat(struct FloatEulers *e, const struct FloatQuat *q)
{
const float qx2 = q->qx * q->qx;
const float qy2 = q->qy * q->qy;
@@ -719,7 +727,7 @@ void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
* @param e Euler output
* @param q Quat input
*/
-void float_eulers_of_quat_yxz(struct FloatEulers *e, struct FloatQuat *q)
+void float_eulers_of_quat_yxz(struct FloatEulers *e, const struct FloatQuat *q)
{
const float qx2 = q->qx * q->qx;
const float qy2 = q->qy * q->qy;
@@ -752,7 +760,7 @@ void float_eulers_of_quat_yxz(struct FloatEulers *e, struct FloatQuat *q)
* @param e Euler output
* @param q Quat input
*/
-void float_eulers_of_quat_zxy(struct FloatEulers *e, struct FloatQuat *q)
+void float_eulers_of_quat_zxy(struct FloatEulers *e, const struct FloatQuat *q)
{
const float qx2 = q->qx * q->qx;
const float qy2 = q->qy * q->qy;
@@ -786,7 +794,7 @@ void float_eulers_of_quat_zxy(struct FloatEulers *e, struct FloatQuat *q)
*
* @return success (0) or not invertible (1)
*/
-bool float_mat_inv_2d(float inv_out[4], float mat_in[4])
+bool float_mat_inv_2d(float inv_out[4], const float mat_in[4])
{
float det = mat_in[0] * mat_in[3] - mat_in[1] * mat_in[2];
@@ -807,7 +815,7 @@ bool float_mat_inv_2d(float inv_out[4], float mat_in[4])
* @param mat[4] Matrix input
* @param vect_in Vector input
*/
-void float_mat2_mult(struct FloatVect2 *vect_out, float mat[4], struct FloatVect2 vect_in)
+void float_mat2_mult(struct FloatVect2 *vect_out, const float mat[4], const struct FloatVect2 vect_in)
{
vect_out->x = mat[0] * vect_in.x + mat[1] * vect_in.y;
vect_out->y = mat[2] * vect_in.x + mat[3] * vect_in.y;
@@ -821,18 +829,18 @@ void float_mat2_mult(struct FloatVect2 *vect_out, float mat[4], struct FloatVect
*
* @return success (0) or not invertible (1)
*/
-bool float_mat_inv_3d(float inv_out[3][3], float mat_in[3][3])
+bool float_mat_inv_3d(float inv_out[3][3], const float mat_in[3][3])
{
- const float m00 = mat_in[1][1]*mat_in[2][2] - mat_in[1][2]*mat_in[2][1];
- const float m10 = mat_in[0][1]*mat_in[2][2] - mat_in[0][2]*mat_in[2][1];
- const float m20 = mat_in[0][1]*mat_in[1][2] - mat_in[0][2]*mat_in[1][1];
- const float m01 = mat_in[1][0]*mat_in[2][2] - mat_in[1][2]*mat_in[2][0];
- const float m11 = mat_in[0][0]*mat_in[2][2] - mat_in[0][2]*mat_in[2][0];
- const float m21 = mat_in[0][0]*mat_in[1][2] - mat_in[0][2]*mat_in[1][0];
- const float m02 = mat_in[1][0]*mat_in[2][1] - mat_in[1][1]*mat_in[2][0];
- const float m12 = mat_in[0][0]*mat_in[2][1] - mat_in[0][1]*mat_in[2][0];
- const float m22 = mat_in[0][0]*mat_in[1][1] - mat_in[0][1]*mat_in[1][0];
- const float det = mat_in[0][0]*m00 - mat_in[1][0]*m10 + mat_in[2][0]*m20;
+ const float m00 = mat_in[1][1] * mat_in[2][2] - mat_in[1][2] * mat_in[2][1];
+ const float m10 = mat_in[0][1] * mat_in[2][2] - mat_in[0][2] * mat_in[2][1];
+ const float m20 = mat_in[0][1] * mat_in[1][2] - mat_in[0][2] * mat_in[1][1];
+ const float m01 = mat_in[1][0] * mat_in[2][2] - mat_in[1][2] * mat_in[2][0];
+ const float m11 = mat_in[0][0] * mat_in[2][2] - mat_in[0][2] * mat_in[2][0];
+ const float m21 = mat_in[0][0] * mat_in[1][2] - mat_in[0][2] * mat_in[1][0];
+ const float m02 = mat_in[1][0] * mat_in[2][1] - mat_in[1][1] * mat_in[2][0];
+ const float m12 = mat_in[0][0] * mat_in[2][1] - mat_in[0][1] * mat_in[2][0];
+ const float m22 = mat_in[0][0] * mat_in[1][1] - mat_in[0][1] * mat_in[1][0];
+ const float det = mat_in[0][0] * m00 - mat_in[1][0] * m10 + mat_in[2][0] * m20;
if (fabs(det) > FLT_EPSILON) {
inv_out[0][0] = m00 / det;
inv_out[1][0] = -m01 / det;
@@ -855,7 +863,7 @@ bool float_mat_inv_3d(float inv_out[3][3], float mat_in[3][3])
* @param mat[3][3] Matrix input
* @param vect_in Vector input
*/
-void float_mat3_mult(struct FloatVect3 *vect_out, float mat[3][3], struct FloatVect3 vect_in)
+void float_mat3_mult(struct FloatVect3 *vect_out, const float mat[3][3], const struct FloatVect3 vect_in)
{
vect_out->x = mat[0][0] * vect_in.x + mat[0][1] * vect_in.y + mat[0][2] * vect_in.z;
vect_out->y = mat[1][0] * vect_in.x + mat[1][1] * vect_in.y + mat[1][2] * vect_in.z;
@@ -867,7 +875,7 @@ void float_mat3_mult(struct FloatVect3 *vect_out, float mat[3][3], struct FloatV
* obtained from: http://rodolphe-vaillant.fr/?e=7
*/
-static float float_mat_minor_4d(float m[4][4], int r0, int r1, int r2, int c0, int c1, int c2)
+static float float_mat_minor_4d(const float m[4][4], int r0, int r1, int r2, int c0, int c1, int c2)
{
return m[r0][c0] * (m[r1][c1] * m[r2][c2] - m[r2][c1] * m[r1][c2]) -
m[r0][c1] * (m[r1][c0] * m[r2][c2] - m[r2][c0] * m[r1][c2]) +
@@ -875,7 +883,7 @@ static float float_mat_minor_4d(float m[4][4], int r0, int r1, int r2, int c0, i
}
-static void float_mat_adjoint_4d(float adjOut[4][4], float m[4][4])
+static void float_mat_adjoint_4d(float adjOut[4][4], const float m[4][4])
{
adjOut[0][0] = float_mat_minor_4d(m, 1, 2, 3, 1, 2, 3);
adjOut[0][1] = -float_mat_minor_4d(m, 0, 2, 3, 1, 2, 3);
@@ -895,7 +903,7 @@ static void float_mat_adjoint_4d(float adjOut[4][4], float m[4][4])
adjOut[3][3] = float_mat_minor_4d(m, 0, 1, 2, 0, 1, 2);
}
-static float float_mat_det_4d(float m[4][4])
+static float float_mat_det_4d(const float m[4][4])
{
return m[0][0] * float_mat_minor_4d(m, 1, 2, 3, 1, 2, 3) -
m[0][1] * float_mat_minor_4d(m, 1, 2, 3, 0, 2, 3) +
@@ -909,7 +917,7 @@ static float float_mat_det_4d(float m[4][4])
* @param invOut output array, inverse of mat_in
* @param mat_in input array
*/
-bool float_mat_inv_4d(float invOut[4][4], float mat_in[4][4])
+bool float_mat_inv_4d(float invOut[4][4], const float mat_in[4][4])
{
float_mat_adjoint_4d(invOut, mat_in);
@@ -932,7 +940,7 @@ bool float_mat_inv_4d(float invOut[4][4], float mat_in[4][4])
Algorithm verified with Matlab.
Thanks to: https://www.quora.com/How-do-I-make-a-C++-program-to-get-the-inverse-of-a-matrix-100-X-100
*/
-void float_mat_invert(float **o, float **mat, int n)
+void float_mat_invert(float **o, float **mat, const int n)
{
int i, j, k;
float t;
@@ -987,7 +995,7 @@ void float_mat_invert(float **o, float **mat, int n)
* Twenty-Five Years Later, SIAM Review, Volume 45, Number 1, March 2003, pages 3-49.
* https://people.sc.fsu.edu/~jburkardt/c_src/matrix_exponential/matrix_exponential.c
*/
-void float_mat_exp(float **a, float **o, int n)
+void float_mat_exp(float **a, float **o, const int n)
{
float a_norm, c, t;
const int q = 6;
@@ -1043,11 +1051,10 @@ void float_mat_exp(float **a, float **o, int n)
for (k = 1; k <= s; k++) {
float_mat_mul_copy(_o, _o, _o, n, n, n);
}
-
}
/* Returns L-oo of matrix a */
-float float_mat_norm_li(float **a, int m, int n)
+float float_mat_norm_li(float **a, const int m, const int n)
{
float row_sum;
float value;
@@ -1064,7 +1071,8 @@ float float_mat_norm_li(float **a, int m, int n)
}
/* Scale a 3D vector to within a 2D bound */
-void float_vect3_bound_in_2d(struct FloatVect3 *vect3, float bound) {
+void float_vect3_bound_in_2d(struct FloatVect3 *vect3, const float bound)
+{
float norm = FLOAT_VECT2_NORM(*vect3);
if (norm > bound) {
float scale = bound / norm;
@@ -1074,10 +1082,11 @@ void float_vect3_bound_in_2d(struct FloatVect3 *vect3, float bound) {
}
/* Scale a 3D vector to within a 3D bound */
-void float_vect3_bound_in_3d(struct FloatVect3 *vect3, float bound) {
+void float_vect3_bound_in_3d(struct FloatVect3 *vect3, const float bound)
+{
float norm = FLOAT_VECT3_NORM(*vect3);
- if(norm>bound) {
- float scale = bound/norm;
+ if (norm > bound) {
+ float scale = bound / norm;
vect3->x *= scale;
vect3->y *= scale;
vect3->z *= scale;
@@ -1085,9 +1094,10 @@ void float_vect3_bound_in_3d(struct FloatVect3 *vect3, float bound) {
}
/* Scale a 3D vector to a certain length in 2D */
-void float_vect3_scale_in_2d(struct FloatVect3 *vect3, float norm_des) {
+void float_vect3_scale_in_2d(struct FloatVect3 *vect3, const float norm_des)
+{
float norm = FLOAT_VECT2_NORM(*vect3);
- if (norm > 0.01f) {
+ if (norm > 0.01f) { // FIXME: magic number
float scale = norm_des / norm;
vect3->x *= scale;
vect3->y *= scale;
@@ -1095,7 +1105,8 @@ void float_vect3_scale_in_2d(struct FloatVect3 *vect3, float norm_des) {
}
/* Scale a 2D vector to within a 2D bound */
-void float_vect2_bound_in_2d(struct FloatVect2 *vect2, float bound) {
+void float_vect2_bound_in_2d(struct FloatVect2 *vect2, const float bound)
+{
float norm = FLOAT_VECT2_NORM(*vect2);
if (norm > bound) {
float scale = bound / norm;
@@ -1105,7 +1116,8 @@ void float_vect2_bound_in_2d(struct FloatVect2 *vect2, float bound) {
}
/* Scale a 2D vector to a certain length in 2D */
-void float_vect2_scale_in_2d(struct FloatVect2 *vect2, float norm_des) {
+void float_vect2_scale_in_2d(struct FloatVect2 *vect2, const float norm_des)
+{
float norm = FLOAT_VECT2_NORM(*vect2);
if (norm > 0.01f) {
float scale = norm_des / norm;
diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h
index b0b611d2c6..b275b961d3 100644
--- a/sw/airborne/math/pprz_algebra_float.h
+++ b/sw/airborne/math/pprz_algebra_float.h
@@ -104,7 +104,7 @@ struct FloatRates {
/*
* Returns the real part of the log of v in base of n
*/
-static inline float float_log_n(float v, float n)
+static inline float float_log_n(const float v, const float n)
{
if (fabsf(v) < 1e-4) { // avoid inf
return - 1.0E+30;
@@ -131,12 +131,12 @@ static inline float float_log_n(float v, float n)
/* macros also usable if _v is not a FloatVect2, but a different struct with x,y members */
#define FLOAT_VECT2_NORM(_v) sqrtf(VECT2_NORM2(_v))
-static inline float float_vect2_norm2(struct FloatVect2 *v)
+static inline float float_vect2_norm2(const struct FloatVect2 *v)
{
return v->x * v->x + v->y * v->y;
}
-static inline float float_vect2_norm(struct FloatVect2 *v)
+static inline float float_vect2_norm(const struct FloatVect2 *v)
{
return sqrtf(float_vect2_norm2(v));
}
@@ -163,12 +163,12 @@ static inline void float_vect2_normalize(struct FloatVect2 *v)
/* macros also usable if _v is not a FloatVect3, but a different struct with x,y,z members */
#define FLOAT_VECT3_NORM(_v) sqrtf(VECT3_NORM2(_v))
-static inline float float_vect3_norm2(struct FloatVect3 *v)
+static inline float float_vect3_norm2(const struct FloatVect3 *v)
{
return v->x * v->x + v->y * v->y + v->z * v->z;
}
-static inline float float_vect3_norm(struct FloatVect3 *v)
+static inline float float_vect3_norm(const struct FloatVect3 *v)
{
return sqrtf(float_vect3_norm2(v));
}
@@ -201,14 +201,17 @@ static inline void float_vect3_normalize(struct FloatVect3 *v)
}
-extern void float_vect3_integrate_fi(struct FloatVect3 *vec, struct FloatVect3 *dv,
- float dt);
+extern void float_vect3_integrate_fi(struct FloatVect3 *vec, const struct FloatVect3 *dv,
+ const float dt);
-extern void float_rates_integrate_fi(struct FloatRates *r, struct FloatRates *dr,
- float dt);
+extern void float_rates_integrate_fi(struct FloatRates *r, const struct FloatRates *dr,
+ const float dt);
-extern void float_rates_of_euler_dot(struct FloatRates *r, struct FloatEulers *e,
- struct FloatEulers *edot);
+extern void float_rates_vect3_integrate_fi(struct FloatRates *r, const struct FloatVect3 *dr,
+ const float dt);
+
+extern void float_rates_of_euler_dot(struct FloatRates *r, const struct FloatEulers *e,
+ const struct FloatEulers *edot);
/* defines for backwards compatibility */
#define FLOAT_VECT3_INTEGRATE_FI(_vo, _dv, _dt) WARNING("FLOAT_VECT3_INTEGRATE_FI macro is deprecated, use the lower case function instead") float_vect3_integrate_fi(&(_vo), &(_dv), _dt)
@@ -260,61 +263,61 @@ static inline void float_rmat_identity(struct FloatRMat *rm)
/** Inverse/transpose of a rotation matrix.
* m_b2a = inv(_m_a2b) = transp(_m_a2b)
*/
-extern void float_rmat_inv(struct FloatRMat *m_b2a, struct FloatRMat *m_a2b);
+extern void float_rmat_inv(struct FloatRMat *m_b2a, const struct FloatRMat *m_a2b);
/** Composition (multiplication) of two rotation matrices.
* m_a2c = m_a2b comp m_b2c , aka m_a2c = m_b2c * m_a2b
*/
-extern void float_rmat_comp(struct FloatRMat *m_a2c, struct FloatRMat *m_a2b,
- struct FloatRMat *m_b2c);
+extern void float_rmat_comp(struct FloatRMat *m_a2c, const struct FloatRMat *m_a2b,
+ const struct FloatRMat *m_b2c);
/** Composition (multiplication) of two rotation matrices.
* m_a2b = m_a2c comp_inv m_b2c , aka m_a2b = inv(_m_b2c) * m_a2c
*/
-extern void float_rmat_comp_inv(struct FloatRMat *m_a2b, struct FloatRMat *m_a2c,
- struct FloatRMat *m_b2c);
+extern void float_rmat_comp_inv(struct FloatRMat *m_a2b, const struct FloatRMat *m_a2c,
+ const struct FloatRMat *m_b2c);
/// Norm of a rotation matrix.
-extern float float_rmat_norm(struct FloatRMat *rm);
+extern float float_rmat_norm(const struct FloatRMat *rm);
/** rotate 3D vector by rotation matrix.
* vb = m_a2b * va
*/
-extern void float_rmat_vmult(struct FloatVect3 *vb, struct FloatRMat *m_a2b,
- struct FloatVect3 *va);
+extern void float_rmat_vmult(struct FloatVect3 *vb, const struct FloatRMat *m_a2b,
+ const struct FloatVect3 *va);
/** rotate 3D vector by transposed rotation matrix.
* vb = m_b2a^T * va
*/
-extern void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a,
- struct FloatVect3 *va);
+extern void float_rmat_transp_vmult(struct FloatVect3 *vb, const struct FloatRMat *m_b2a,
+ const struct FloatVect3 *va);
/** rotate angle by rotation matrix.
* rb = m_a2b * ra
*/
-extern void float_rmat_mult(struct FloatEulers *rb, struct FloatRMat *m_a2b,
- struct FloatEulers *ra);
+extern void float_rmat_mult(struct FloatEulers *rb, const struct FloatRMat *m_a2b,
+ const struct FloatEulers *ra);
/** rotate angle by transposed rotation matrix.
* rb = m_b2a^T * ra
*/
-extern void float_rmat_transp_mult(struct FloatEulers *rb, struct FloatRMat *m_b2a,
- struct FloatEulers *ra);
+extern void float_rmat_transp_mult(struct FloatEulers *rb, const struct FloatRMat *m_b2a,
+ const struct FloatEulers *ra);
/** rotate anglular rates by rotation matrix.
* rb = m_a2b * ra
*/
-extern void float_rmat_ratemult(struct FloatRates *rb, struct FloatRMat *m_a2b,
- struct FloatRates *ra);
+extern void float_rmat_ratemult(struct FloatRates *rb, const struct FloatRMat *m_a2b,
+ const struct FloatRates *ra);
/** rotate anglular rates by transposed rotation matrix.
* rb = m_b2a^T * ra
*/
-extern void float_rmat_transp_ratemult(struct FloatRates *rb, struct FloatRMat *m_b2a,
- struct FloatRates *ra);
+extern void float_rmat_transp_ratemult(struct FloatRates *rb, const struct FloatRMat *m_b2a,
+ const struct FloatRates *ra);
/** initialises a rotation matrix from unit vector axis and angle */
-extern void float_rmat_of_axis_angle(struct FloatRMat *rm, struct FloatVect3 *uv, float angle);
+extern void float_rmat_of_axis_angle(struct FloatRMat *rm, const struct FloatVect3 *uv, const float angle);
/** Rotation matrix from 321 Euler angles (float).
* The Euler angles are interpreted as zy'x'' (intrinsic) rotation.
@@ -328,13 +331,13 @@ extern void float_rmat_of_axis_angle(struct FloatRMat *rm, struct FloatVect3 *uv
* @param[out] rm pointer to rotation matrix
* @param[in] e pointer to Euler angles
*/
-extern void float_rmat_of_eulers_321(struct FloatRMat *rm, struct FloatEulers *e);
-extern void float_rmat_of_eulers_312(struct FloatRMat *rm, struct FloatEulers *e);
+extern void float_rmat_of_eulers_321(struct FloatRMat *rm, const struct FloatEulers *e);
+extern void float_rmat_of_eulers_312(struct FloatRMat *rm, const struct FloatEulers *e);
#define float_rmat_of_eulers float_rmat_of_eulers_321
-extern void float_rmat_of_quat(struct FloatRMat *rm, struct FloatQuat *q);
+extern void float_rmat_of_quat(struct FloatRMat *rm, const struct FloatQuat *q);
/** in place first order integration of a rotation matrix */
-extern void float_rmat_integrate_fi(struct FloatRMat *rm, struct FloatRates *omega, float dt);
+extern void float_rmat_integrate_fi(struct FloatRMat *rm, const struct FloatRates *omega, const float dt);
extern float float_rmat_reorthogonalize(struct FloatRMat *rm);
/* defines for backwards compatibility */
@@ -372,7 +375,7 @@ static inline void float_quat_identity(struct FloatQuat *q)
#define FLOAT_QUAT_NORM2(_q) (SQUARE((_q).qi) + SQUARE((_q).qx) + SQUARE((_q).qy) + SQUARE((_q).qz))
-static inline float float_quat_norm(struct FloatQuat *q)
+static inline float float_quat_norm(const struct FloatQuat *q)
{
return sqrtf(SQUARE(q->qi) + SQUARE(q->qx) + SQUARE(q->qy) + SQUARE(q->qz));
}
@@ -388,7 +391,7 @@ static inline void float_quat_normalize(struct FloatQuat *q)
}
}
-static inline void float_quat_invert(struct FloatQuat *qo, struct FloatQuat *qi)
+static inline void float_quat_invert(struct FloatQuat *qo, const struct FloatQuat *qi)
{
QUAT_INVERT(*qo, *qi);
}
@@ -406,72 +409,75 @@ static inline void float_quat_wrap_shortest(struct FloatQuat *q)
/** Composition (multiplication) of two quaternions.
* a2c = a2b comp b2c , aka a2c = a2b * b2c
*/
-extern void float_quat_comp(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c);
+extern void float_quat_comp(struct FloatQuat *a2c, const struct FloatQuat *a2b, const struct FloatQuat *b2c);
/** Composition (multiplication) of two quaternions.
* a2b = a2c comp_inv b2c , aka a2b = a2c * inv(b2c)
*/
-extern void float_quat_comp_inv(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c);
+extern void float_quat_comp_inv(struct FloatQuat *a2b, const struct FloatQuat *a2c, const struct FloatQuat *b2c);
/** Composition (multiplication) of two quaternions.
* b2c = a2b inv_comp a2c , aka b2c = inv(_a2b) * a2c
*/
-extern void float_quat_inv_comp(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c);
+extern void float_quat_inv_comp(struct FloatQuat *b2c, const struct FloatQuat *a2b, const struct FloatQuat *a2c);
/** Composition (multiplication) of two quaternions with normalization.
* a2c = a2b comp b2c , aka a2c = a2b * b2c
*/
-extern void float_quat_comp_norm_shortest(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c);
+extern void float_quat_comp_norm_shortest(struct FloatQuat *a2c, const struct FloatQuat *a2b,
+ const struct FloatQuat *b2c);
/** Composition (multiplication) of two quaternions with normalization.
* a2b = a2c comp_inv b2c , aka a2b = a2c * inv(b2c)
*/
-extern void float_quat_comp_inv_norm_shortest(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c);
+extern void float_quat_comp_inv_norm_shortest(struct FloatQuat *a2b, const struct FloatQuat *a2c,
+ const struct FloatQuat *b2c);
/** Composition (multiplication) of two quaternions with normalization.
* b2c = a2b inv_comp a2c , aka b2c = inv(_a2b) * a2c
*/
-extern void float_quat_inv_comp_norm_shortest(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c);
+extern void float_quat_inv_comp_norm_shortest(struct FloatQuat *b2c, const struct FloatQuat *a2b,
+ const struct FloatQuat *a2c);
/** Quaternion derivative from rotational velocity.
* qd = -0.5*omega(r) * q
* or equally:
* qd = 0.5 * q * omega(r)
*/
-extern void float_quat_derivative(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q);
+extern void float_quat_derivative(struct FloatQuat *qd, const struct FloatRates *r, const struct FloatQuat *q);
/** Quaternion derivative from rotational velocity with Lagrange multiplier.
* qd = -0.5*omega(r) * q
* or equally:
* qd = 0.5 * q * omega(r)
*/
-extern void float_quat_derivative_lagrange(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q);
+extern void float_quat_derivative_lagrange(struct FloatQuat *qd, const struct FloatRates *r, const struct FloatQuat *q);
/** Delta rotation quaternion with constant angular rates.
*/
-extern void float_quat_differential(struct FloatQuat *q_out, struct FloatRates *w, float dt);
+extern void float_quat_differential(struct FloatQuat *q_out, const struct FloatRates *w, const float dt);
/** in place first order quaternion integration with constant rotational velocity */
-extern void float_quat_integrate_fi(struct FloatQuat *q, struct FloatRates *omega, float dt);
+extern void float_quat_integrate_fi(struct FloatQuat *q, const struct FloatRates *omega, const float dt);
/** in place quaternion integration with constant rotational velocity */
-extern void float_quat_integrate(struct FloatQuat *q, struct FloatRates *omega, float dt);
+extern void float_quat_integrate(struct FloatQuat *q, const struct FloatRates *omega, const float dt);
/** rotate 3D vector by quaternion.
* vb = q_a2b * va * q_a2b^-1
*/
-extern void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q, const struct FloatVect3 *v_in);
+extern void float_quat_vmult(struct FloatVect3 *v_out, const struct FloatQuat *q, const struct FloatVect3 *v_in);
/// Quaternion from Euler angles.
-extern void float_quat_of_eulers(struct FloatQuat *q, struct FloatEulers *e);
-extern void float_quat_of_eulers_zxy(struct FloatQuat *q, struct FloatEulers *e);
-extern void float_quat_of_eulers_yxz(struct FloatQuat *q, struct FloatEulers *e);
+extern void float_quat_of_eulers(struct FloatQuat *q, const struct FloatEulers *e);
+extern void float_quat_of_eulers_zxy(struct FloatQuat *q, const struct FloatEulers *e);
+extern void float_quat_of_eulers_yxz(struct FloatQuat *q, const struct FloatEulers *e);
/** Quaternion from unit vector and angle.
* Output quaternion is not normalized.
* It will be a unit quaternion only if the input vector is also unitary.
*/
-extern void float_quat_of_axis_angle(struct FloatQuat *q, const struct FloatVect3 *uv, float angle);
+extern void float_quat_of_axis_angle(struct FloatQuat *q, const struct FloatVect3 *uv, const float angle);
/** Quaternion from orientation vector.
* Length/norm of the vector is the angle.
@@ -479,10 +485,10 @@ extern void float_quat_of_axis_angle(struct FloatQuat *q, const struct FloatVect
extern void float_quat_of_orientation_vect(struct FloatQuat *q, const struct FloatVect3 *ov);
/// Quaternion from rotation matrix.
-extern void float_quat_of_rmat(struct FloatQuat *q, struct FloatRMat *rm);
+extern void float_quat_of_rmat(struct FloatQuat *q, const struct FloatRMat *rm);
/// Tilt twist decomposition of quaternion
-extern void float_quat_tilt_twist(struct FloatQuat *tilt, struct FloatQuat *twist, struct FloatQuat *quat);
+extern void float_quat_tilt_twist(struct FloatQuat *tilt, struct FloatQuat *twist, const struct FloatQuat *quat);
/* defines for backwards compatibility */
@@ -518,14 +524,14 @@ extern void float_quat_tilt_twist(struct FloatQuat *tilt, struct FloatQuat *twis
#define FLOAT_EULERS_ZERO(_e) EULERS_ASSIGN(_e, 0., 0., 0.);
-static inline float float_eulers_norm(struct FloatEulers *e)
+static inline float float_eulers_norm(const struct FloatEulers *e)
{
return sqrtf(SQUARE(e->phi) + SQUARE(e->theta) + SQUARE(e->psi));
}
-extern void float_eulers_of_rmat(struct FloatEulers *e, struct FloatRMat *rm);
-extern void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q);
-extern void float_eulers_of_quat_zxy(struct FloatEulers *e, struct FloatQuat *q);
-extern void float_eulers_of_quat_yxz(struct FloatEulers *e, struct FloatQuat *q);
+extern void float_eulers_of_rmat(struct FloatEulers *e, const struct FloatRMat *rm);
+extern void float_eulers_of_quat(struct FloatEulers *e, const struct FloatQuat *q);
+extern void float_eulers_of_quat_zxy(struct FloatEulers *e, const struct FloatQuat *q);
+extern void float_eulers_of_quat_yxz(struct FloatEulers *e, const struct FloatQuat *q);
/* defines for backwards compatibility */
#define FLOAT_EULERS_OF_RMAT(_e, _rm) WARNING("FLOAT_EULERS_OF_RMAT macro is deprecated, use the lower case function instead") float_eulers_of_rmat(&(_e), &(_rm))
@@ -642,12 +648,12 @@ static inline float float_vect_dot_product(const float *a, const float *b, const
for (__i = 0; __i < _rows; __i++) { _ptr[__i] = &_mat[__i][0]; } \
}
-extern void float_mat_invert(float **o, float **mat, int n);
-extern void float_mat_exp(float **a, float **o, int n);
-extern float float_mat_norm_li(float **o, int m, int n);
+extern void float_mat_invert(float **o, float **mat, const int n);
+extern void float_mat_exp(float **a, float **o, const int n);
+extern float float_mat_norm_li(float **o, const int m, const int n);
/** a = 0 */
-static inline void float_mat_zero(float **a, int m, int n)
+static inline void float_mat_zero(float **a, const int m, const int n)
{
int i, j;
for (i = 0; i < m; i++) {
@@ -656,7 +662,7 @@ static inline void float_mat_zero(float **a, int m, int n)
}
/** a = b */
-static inline void float_mat_copy(float **a, float **b, int m, int n)
+static inline void float_mat_copy(float **a, float **b, const int m, const int n)
{
int i, j;
for (i = 0; i < m; i++) {
@@ -667,7 +673,7 @@ static inline void float_mat_copy(float **a, float **b, int m, int n)
/** o = a + b */
-static inline void float_mat_sum(float **o, float **a, float **b, int m, int n)
+static inline void float_mat_sum(float **o, float **a, float **b, const int m, const int n)
{
int i, j;
for (i = 0; i < m; i++) {
@@ -676,7 +682,7 @@ static inline void float_mat_sum(float **o, float **a, float **b, int m, int n)
}
/** o = a - b */
-static inline void float_mat_diff(float **o, float **a, float **b, int m, int n)
+static inline void float_mat_diff(float **o, float **a, float **b, const int m, const int n)
{
int i, j;
for (i = 0; i < m; i++) {
@@ -685,7 +691,7 @@ static inline void float_mat_diff(float **o, float **a, float **b, int m, int n)
}
/** transpose square matrix */
-static inline void float_mat_transpose_square(float **a, int n)
+static inline void float_mat_transpose_square(float **a, const int n)
{
int i, j;
for (i = 0; i < n; i++) {
@@ -699,7 +705,7 @@ static inline void float_mat_transpose_square(float **a, int n)
/** transpose non-square matrix */
-static inline void float_mat_transpose(float **o, float **a, int n, int m)
+static inline void float_mat_transpose(float **o, float **a, const int n, const int m)
{
int i, j;
for (i = 0; i < n; i++) {
@@ -715,7 +721,7 @@ static inline void float_mat_transpose(float **o, float **a, int n, int m)
* b: [n x l]
* o: [m x l]
*/
-static inline void float_mat_mul(float **o, float **a, float **b, int m, int n, int l)
+static inline void float_mat_mul(float **o, float **a, float **b, const int m, const int n, const int l)
{
int i, j, k;
for (i = 0; i < m; i++) {
@@ -734,7 +740,7 @@ static inline void float_mat_mul(float **o, float **a, float **b, int m, int n,
* b: [l x n]
* o: [m x l]
*/
-static inline void float_mat_mul_transpose(float **o, float **a, float **b, int m, int n, int l)
+static inline void float_mat_mul_transpose(float **o, float **a, float **b, const int m, const int n, const int l)
{
int i, j, k;
for (i = 0; i < m; i++) {
@@ -758,7 +764,7 @@ static inline void float_mat_mul_transpose(float **o, float **a, float **b, int
* of the input matrices when this function is used, which is useful for consecutive multiplications
* (e.g. when doing matrix exponentiation), at the cost of some copy overhead.
*/
-static inline void float_mat_mul_copy(float **o, float **a, float **b, int m, int n, int l)
+static inline void float_mat_mul_copy(float **o, float **a, float **b, const int m, const int n, const int l)
{
float temp[m][l];
int i, j, k;
@@ -782,7 +788,7 @@ static inline void float_mat_mul_copy(float **o, float **a, float **b, int m, in
* b: [n x 1]
* o: [m x 1]
*/
-static inline void float_mat_vect_mul(float *o, float **a, float *b, int m, int n)
+static inline void float_mat_vect_mul(float *o, float **a, const float *b, const int m, const int n)
{
int i, j;
for (i = 0; i < m; i++) {
@@ -798,7 +804,7 @@ static inline void float_mat_vect_mul(float *o, float **a, float *b, int m, int
* a: [m x n]
* k: [1 x 1]
*/
-static inline void float_mat_scale(float **a, float k, int m, int n)
+static inline void float_mat_scale(float **a, float k, const int m, const int n)
{
int i, j;
for (i = 0; i < m; i++) {
@@ -814,7 +820,7 @@ static inline void float_mat_scale(float **a, float k, int m, int n)
* b: [m x n]
* k: [1 x 1]
*/
-static inline void float_mat_sum_scaled(float **a, float **b, float k, int m, int n)
+static inline void float_mat_sum_scaled(float **a, float **b, const float k, const int m, const int n)
{
int i, j;
for (i = 0; i < m; i++) {
@@ -831,7 +837,7 @@ static inline void float_mat_sum_scaled(float **a, float **b, float k, int m, in
* o: [I(d,d) 0 ]
* [ 0 a(d,m:d,n)]
*/
-static inline void float_mat_minor(float **o, float **a, int m, int n, int d)
+static inline void float_mat_minor(float **o, float **a, const int m, const int n, const int d)
{
int i, j;
float_mat_zero(o, m, n);
@@ -844,7 +850,7 @@ static inline void float_mat_minor(float **o, float **a, int m, int n, int d)
}
/** o = I - v v^T */
-static inline void float_mat_vmul(float **o, float *v, int n)
+static inline void float_mat_vmul(float **o, const float *v, const int n)
{
int i, j;
for (i = 0; i < n; i++) {
@@ -858,7 +864,7 @@ static inline void float_mat_vmul(float **o, float *v, int n)
}
/** o = c-th column of matrix a[m x n] */
-static inline void float_mat_col(float *o, float **a, int m, int c)
+static inline void float_mat_col(float *o, float **a, const int m, const int c)
{
int i;
for (i = 0; i < m; i++) {
@@ -867,7 +873,7 @@ static inline void float_mat_col(float *o, float **a, int m, int c)
}
/** Make an n x n identity matrix (for matrix passed as array) */
-static inline void float_mat_diagonal_scal(float **o, float v, int n)
+static inline void float_mat_diagonal_scal(float **o, float v, const int n)
{
int i, j;
for (i = 0 ; i < n; i++) {
@@ -883,41 +889,40 @@ static inline void float_mat_diagonal_scal(float **o, float v, int n)
/** Divide a matrix by a scalar */
-static inline void float_mat_div_scalar(float **o, float **a, float scalar, int m, int n)
+static inline void float_mat_div_scalar(float **o, float **a, const float scalar, const int m, const int n)
{
- int i, j;
- for (i = 0; i < m; i++) {
- for (j = 0; j < n; j++) {
- o[i][j] = a[i][j] / scalar;
- }
- }
+ int i, j;
+ for (i = 0; i < m; i++) {
+ for (j = 0; j < n; j++) {
+ o[i][j] = a[i][j] / scalar;
+ }
+ }
}
/** Multiply a matrix by a scalar */
-static inline void float_mat_mul_scalar(float **o, float **a, float scalar, int m, int n)
+static inline void float_mat_mul_scalar(float **o, float **a, const float scalar, const int m, const int n)
{
- int i, j;
- for (i = 0; i < m; i++) {
- for (j = 0; j < n; j++) {
- o[i][j] = a[i][j] * scalar;
- }
- }
+ int i, j;
+ for (i = 0; i < m; i++) {
+ for (j = 0; j < n; j++) {
+ o[i][j] = a[i][j] * scalar;
+ }
+ }
}
-extern bool float_mat_inv_2d(float inv_out[4], float mat_in[4]);
-extern void float_mat2_mult(struct FloatVect2 *vect_out, float mat[4], struct FloatVect2 vect_in);
-extern bool float_mat_inv_3d(float inv_out[3][3], float mat_in[3][3]);
-extern void float_mat3_mult(struct FloatVect3 *vect_out, float mat[3][3], struct FloatVect3 vect_in);
-extern bool float_mat_inv_4d(float invOut[4][4], float mat_in[4][4]);
-
-extern void float_vect3_bound_in_2d(struct FloatVect3 *vect3, float bound);
-extern void float_vect3_bound_in_3d(struct FloatVect3 *vect3, float bound);
-extern void float_vect3_scale_in_2d(struct FloatVect3 *vect3, float norm_des);
-extern void float_vect2_bound_in_2d(struct FloatVect2 *vect2, float bound);
-extern void float_vect2_scale_in_2d(struct FloatVect2 *vect2, float norm_des);
+extern bool float_mat_inv_2d(float inv_out[4], const float mat_in[4]);
+extern void float_mat2_mult(struct FloatVect2 *vect_out, const float mat[4], const struct FloatVect2 vect_in);
+extern bool float_mat_inv_3d(float inv_out[3][3], const float mat_in[3][3]);
+extern void float_mat3_mult(struct FloatVect3 *vect_out, const float mat[3][3], const struct FloatVect3 vect_in);
+extern bool float_mat_inv_4d(float invOut[4][4], const float mat_in[4][4]);
+extern void float_vect3_bound_in_2d(struct FloatVect3 *vect3, const float bound);
+extern void float_vect3_bound_in_3d(struct FloatVect3 *vect3, const float bound);
+extern void float_vect3_scale_in_2d(struct FloatVect3 *vect3, const float norm_des);
+extern void float_vect2_bound_in_2d(struct FloatVect2 *vect2, const float bound);
+extern void float_vect2_scale_in_2d(struct FloatVect2 *vect2, const float norm_des);
#ifdef __cplusplus
} /* extern "C" */
#endif
diff --git a/sw/airborne/state.h b/sw/airborne/state.h
index 016ec29c64..19ca094150 100644
--- a/sw/airborne/state.h
+++ b/sw/airborne/state.h
@@ -1097,6 +1097,14 @@ static inline struct Int32Vect3 *stateGetAccelBody_i(void)
}
/** @}*/
+static inline struct FloatVect3 *stateGetAccelBody_f(void)
+{
+ static struct FloatVect3 body_accel_f;
+ body_accel_f.x = FLOAT_OF_BFP(state.body_accel_i.x, INT32_ACCEL_FRAC);
+ body_accel_f.y = FLOAT_OF_BFP(state.body_accel_i.y, INT32_ACCEL_FRAC);
+ body_accel_f.z = FLOAT_OF_BFP(state.body_accel_i.z, INT32_ACCEL_FRAC);
+ return &body_accel_f;
+}
/******************************************************************************
diff --git a/sw/include/std.h b/sw/include/std.h
index 16523b6fbf..3185f9ce73 100644
--- a/sw/include/std.h
+++ b/sw/include/std.h
@@ -31,7 +31,7 @@
#include
#ifdef SITL
-#include // for debuging in simulation
+ #include // for debuging in simulation
#endif
/* some convenience macros to print debug/config messages at compile time */
@@ -44,46 +44,46 @@
#define PTR(_f) &_f
#ifndef FALSE
-#define FALSE false
+ #define FALSE false
#endif
#ifndef TRUE
-#define TRUE true
+ #define TRUE true
#endif
#ifndef NULL
-#ifdef __cplusplus
-#define NULL 0
-#else
-#define NULL ((void *)0)
-#endif
+ #ifdef __cplusplus
+ #define NULL 0
+ #else
+ #define NULL ((void *)0)
+ #endif
#endif
/* Unit (void) values */
typedef uint8_t unit_t;
#ifndef M_PI
-#define M_PI 3.14159265358979323846
+ #define M_PI 3.14159265358979323846
#endif
#ifndef M_PI_6
-#define M_PI_6 (M_PI/6)
+ #define M_PI_6 (M_PI/6)
#endif
#ifndef M_PI_4
-#define M_PI_4 (M_PI/4)
+ #define M_PI_4 (M_PI/4)
#endif
#ifndef M_PI_2
-#define M_PI_2 (M_PI/2)
+ #define M_PI_2 (M_PI/2)
#endif
#ifndef bit_is_set
-#define bit_is_set(x, b) ((x >> b) & 0x1)
+ #define bit_is_set(x, b) ((x >> b) & 0x1)
#endif
#ifndef _BV
-#define _BV(bit) (1 << (bit))
+ #define _BV(bit) (1 << (bit))
#endif
#define SetBit(a, n) a |= (1 << n)
@@ -122,6 +122,7 @@ typedef uint8_t unit_t;
#define DeciDegOfRad(x) ((x) * (1800./ M_PI))
#define RadOfDeg(x) ((x) * (M_PI/180.))
#define RadOfDeciDeg(x) ((x) * (M_PI/1800.))
+#define RadOfCentiDeg(x) ((x) * (M_PI/18000.))
#define MOfCm(_x) (((float)(_x))/100.)
#define MOfMm(_x) (((float)(_x))/1000.)
@@ -134,7 +135,7 @@ typedef uint8_t unit_t;
#define MoreThan(_x, _y) ((_x) > (_y))
#ifndef ABS
-#define ABS(val) ((val) < 0 ? -(val) : (val))
+ #define ABS(val) ((val) < 0 ? -(val) : (val))
#endif
#define BoundUpper(_x, _max) { if (_x > (_max)) _x = (_max);}
@@ -270,17 +271,17 @@ static inline bool str_equal(const char *a, const char *b)
}
#ifdef __GNUC__
-# define UNUSED __attribute__((__unused__))
-# define WEAK __attribute__((weak))
+ #define UNUSED __attribute__((__unused__))
+ #define WEAK __attribute__((weak))
#else
-# define UNUSED
-# define WEAK
+ #define UNUSED
+ #define WEAK
#endif
#if __GNUC__ >= 7
-# define INTENTIONAL_FALLTHRU __attribute__ ((fallthrough));
+ #define INTENTIONAL_FALLTHRU __attribute__ ((fallthrough));
#else
-# define INTENTIONAL_FALLTHRU
+ #define INTENTIONAL_FALLTHRU
#endif
#endif /* STD_H */