Ilyhadev/gimbal (#128)

Author: Ilyhadev

Libs/Dronecan

@@ -8,7 +8,6 @@
 #include <math.h>
 #include <algorithm>
 
-
 PwmDurationUs mapInt16CommandToPwm(int16_t command,
                                    PwmDurationUs min_pwm,
                                    PwmDurationUs max_pwm,
@@ -94,3 +93,144 @@ float AdaptiveAlphaFilter::linearly_interpolate_alpha(float delta) const {
 
     return alpha;
 }
+
+void rad_to_deg_array(float angles_rpy[3]) {
+    constexpr float RAD_TO_DEG = 57.2957795131f;  // 180.0f / PI
+    angles_rpy[0] *= RAD_TO_DEG;
+    angles_rpy[1] *= RAD_TO_DEG;
+    angles_rpy[2] *= RAD_TO_DEG;
+}
+
+float fast_atan2(float y, float x) {
+    if (std::abs(x) < FLT_EPSILON_LOCAL && std::abs(y) < FLT_EPSILON_LOCAL) {
+        return 0.0f;
+    }
+
+    float angle = 0.0f;
+    if (x < 0.0f) {
+        angle = (y >= 0.0f) ? PI : -PI;
+        // Effectively rotating by 180 degrees
+        x = -x;
+        y = -y;
+    }
+
+    // Iterative CORDIC rotation
+    for (int i = 0; i < 12; i++) {
+        float x_new;
+        float sigma = (y >= 0.0f) ? 1.0f : -1.0f;
+        float factor = std::ldexp(1.0f, -i);  // 2^-i
+
+        x_new = x + (sigma * y * factor);
+        y = y - (sigma * x * factor);
+        x = x_new;
+
+        angle += sigma * CORDIC_TABLE[i];
+    }
+
+    return angle;
+}
+
+// ZYX "decoding" (yaw-pitch-roll) convention
+void quaternion_to_euler(const float q[4],
+                         float angles_rpy[3]) {
+    const float qx = q[0];
+    const float qy = q[1];
+    const float qz = q[2];
+    const float qw = q[3];
+
+    // Calculate the Sine of Pitch
+    float sinp = 2.0f * (qw * qy - qx * qz);
+
+    // Prevents NaN in sqrt/atan2 if the quaternion is slightly denormalized
+    if (sinp > 1.0f) sinp = 1.0f;
+    else if (sinp < -1.0f) sinp = -1.0f;
+
+    // Gimbal Lock Check
+    if (std::abs(sinp) > 0.9995f) {
+        angles_rpy[0] = 0.0f;
+        angles_rpy[1] = std::copysign(PI_2, sinp);
+        angles_rpy[2] = fast_atan2(2.0f * (qx * qy + qw * qz),
+                                   1.0f - 2.0f * (qy * qy + qz * qz));
+    } else {
+        // Roll (x-axis)
+        angles_rpy[0] = fast_atan2(2.0f * (qw * qx + qy * qz),
+                                   1.0f - 2.0f * (qx * qx + qy * qy));
+
+        // Pitch (y-axis)
+        float cosp = std::sqrt(1.0f - sinp * sinp);
+        angles_rpy[1] = fast_atan2(sinp, cosp);
+
+        // Yaw (z-axis)
+        angles_rpy[2] = fast_atan2(2.0f * (qw * qz + qx * qy),
+                                   1.0f - 2.0f * (qy * qy + qz * qz));
+    }
+}
+
+void normalize_quaternion(float q[4]) {
+    float norm_sq = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3];
+
+    if (norm_sq > 0.0f && std::fabs(norm_sq - 1.0f) > 1e-3f) {
+        float inv_norm = 1.0f / std::sqrt(norm_sq);
+        q[0] *= inv_norm;
+        q[1] *= inv_norm;
+        q[2] *= inv_norm;
+        q[3] *= inv_norm;
+    }
+}
+
+void fast_sin_cos(float theta_rad, float* s, float* c) {
+    // Range Reduction. Works on [-PI, PI]
+    bool flip = false;
+    if (theta_rad > PI_2) {
+        theta_rad = PI - theta_rad;
+        flip = true;
+    } else if (theta_rad < -PI_2) {
+        theta_rad = -PI - theta_rad;
+        flip = true;
+    }
+
+    float x = CORDIC_GAIN;
+    float y = 0.0f;
+    float current_theta = 0.0f;
+
+    // Iterative CORDIC rotation
+    for (int i = 0; i < 12; i++) {
+        float x_new;
+        float sigma = (theta_rad >= current_theta) ? 1.0f : -1.0f;
+        float factor = std::ldexp(1.0f, -i);  // 2^-i
+
+        x_new = x - (sigma * y * factor);
+        y = y + (sigma * x * factor);
+        x = x_new;
+
+        current_theta += sigma * CORDIC_TABLE[i];
+    }
+
+    // Quadrant Adjustment
+    if (flip) {
+        *c = -x;
+        *s = y;
+    } else {
+        *c = x;
+        *s = y;
+    }
+}
+
+void euler_to_quaternion(float const angles_rpy[3], float q[4]) {
+    // Conversion factor deg to rad and halving for quaternion math
+    constexpr float DEG_TO_RAD_HALF = (PI / 180.0f) * 0.5f;
+
+    float sr, cr, sp, cp, sy, cy;
+
+    // Compute sin/cos for half-angles
+    fast_sin_cos(angles_rpy[0] * DEG_TO_RAD_HALF, &sr, &cr);
+    fast_sin_cos(angles_rpy[1] * DEG_TO_RAD_HALF, &sp, &cp);
+    fast_sin_cos(angles_rpy[2] * DEG_TO_RAD_HALF, &sy, &cy);
+
+    // ZYX Convention
+    // q = [x, y, z, w]
+    q[0] = sr * cp * cy - cr * sp * sy;  // x
+    q[1] = cr * sp * cy + sr * cp * sy;  // y
+    q[2] = cr * cp * sy - sr * sp * cy;  // z
+    q[3] = cr * cp * cy + sr * sp * sy;  // w
+}

Src/common/algorithms.cpp

@@ -9,7 +9,21 @@
 
 #include <stdint.h>
 
-typedef uint16_t PwmDurationUs;
+using PwmDurationUs = uint16_t;
+static constexpr float PI_2 = 1.5707963267f;
+static constexpr float LOOSE_ERR = 1e-3f;
+static constexpr float PI = 3.1415926535f;
+static constexpr float CORDIC_GAIN = 0.607252935f;
+// Using a small epsilon for float comparisons
+static constexpr float FLT_EPSILON_LOCAL = 1e-7f;
+
+// Just precomputed arctan(2^-i) values for i = 0 to 11
+static const float CORDIC_TABLE[] = {
+    0.785398163f, 0.463647609f, 0.244978663f, 0.124354995f,
+    0.062418810f, 0.031239833f, 0.015623729f, 0.007812341f,
+    0.003906230f, 0.001953123f, 0.000976562f, 0.000488281f
+};
+
 
 /**
  * @brief Maps a 16-bit integer command to a PWM duration.
@@ -58,6 +72,39 @@ float mapPwmToPct(uint16_t pwm_val, int16_t pwm_min, int16_t pwm_max);
 
 void movingAverage(float* prev_avg, float crnt_val, uint16_t size);
 
+/**
+ * @brief Normalizes a quaternion to unit length
+ * @param q Quaternion to normalize in-place [x, y, z, w]
+ */
+void normalize_quaternion(float q[4]);
+
+/**
+ * @brief Converts quaternion to Euler angles (roll, pitch, yaw)
+ * @param q Quaternion in [x, y, z, w] order (body frame)
+ * @param angles_rpy Output array for [roll, pitch, yaw] in radians
+ */
+void quaternion_to_euler(const float q[4],
+                                float angles_rpy[3]);
+
+/** 
+ * @brief Converts radians to degrees for an array of three angles
+ */
+
+ /**
+ * @brief Converts Euler angles (degrees) to a quaternion.
+ * @param roll_deg, pitch_deg, yaw_deg Input angles in degrees.
+ * @param q Output array for quaternion in [x, y, z, w] order.
+ */
+void euler_to_quaternion(float const angles_rpy[3], float q[4]);
+
+/**
+ * @brief Optimized CORDIC sin/cos calculation.
+ * @param theta_rad Input angle in radians.
+ * @param s, c Pointers to output sine and cosine.
+ */
+void fast_sin_cos(float theta_rad, float* s, float* c);
+
+void rad_to_deg_array(float angles_rpy[3]);
 /**
  * @brief The Adaptive Alpha Filter is a variation of the exponential smoothing filter,
  * where the smoothing factor α is adjusted dynamically based on the magnitude of changes

Src/common/algorithms.hpp

@@ -43,7 +43,7 @@ int8_t Mpu9250::read_gyroscope(SensorGyro* gyro) const {
     return 0;
 }
 
-int8_t Mpu9250::read_magnetometer(SensorGyro* mag) const {
+int8_t Mpu9250::read_magnetometer(SensorMag* mag) const {
     (*mag)[0] = 0.0;
     (*mag)[1] = 0.0;
     (*mag)[2] = 0.0;

Src/drivers/mpu9250/mpu9250.cpp

@@ -38,17 +38,17 @@ class Mpu9250 {
     /**
      * @return 0 on success, negative otherwise
      */
-    int8_t read_accelerometer(std::array<int16_t, 3>* accel) const;
+    int8_t read_accelerometer(SensorAccel* accel) const;
 
     /**
      * @return 0 on success, negative otherwise
      */
-    int8_t read_gyroscope(std::array<int16_t, 3>* gyro) const;
+    int8_t read_gyroscope(SensorGyro* gyro) const;
 
     /**
      * @return 0 on success, negative otherwise
      */
-    int8_t read_magnetometer(std::array<int16_t, 3>* mag) const;
+    int8_t read_magnetometer(SensorMag* mag) const;
 
     /**
      * @return 0 on success, negative otherwise

Src/drivers/mpu9250/mpu9250.hpp

@@ -8,8 +8,10 @@
 #include "rcpwm.hpp"
 #include <array>
 #include <algorithm>
+#include <cstdio>
 #include "params.hpp"
 #include "common/algorithms.hpp"
+#include "common/logging.hpp"
 
 int8_t Driver::RCPWM::init() {
     for (const auto& param : channels) {
@@ -45,6 +47,18 @@ uint8_t Driver::RCPWM::get_pin_percent(uint8_t pin_idx) {
         (channels[pin_idx].pin, channels[pin_idx].min, channels[pin_idx].max);
 }
 
+float Driver::RCPWM::get_current_angle(uint16_t max_servo_angle, uint8_t pin_idx) {
+    if (!is_pin_enabled(pin_idx)) return 0.0f;
+
+    auto current_us = HAL::Pwm::get_duration(channels[pin_idx].pin);
+    float normalized = mapFloat(current_us,
+                                channels[pin_idx].min,
+                                channels[pin_idx].max,
+                                -1.0f, +1.0f);
+
+    return normalized * (static_cast<float>(max_servo_angle) / 2.0f);
+}
+
 int8_t Driver::RCPWM::get_pin_channel(uint8_t pin_idx) {
     return pin_idx < get_pins_amount() ? channels[pin_idx].channel : -1;
 }

Src/drivers/rcpwm/rcpwm.cpp

@@ -112,7 +112,7 @@ class RCPWM {
 
     static int8_t get_pin_channel(uint8_t pin_idx);
     static bool is_pin_enabled(uint8_t pin_idx);
-
+    static float get_current_angle(uint16_t max_angle_deg, uint8_t pin_idx);
 // private:
     static std::array<RcpwmChannel, static_cast<uint8_t>(HAL::PwmPin::PWM_AMOUNT)> channels;
 };

Src/drivers/rcpwm/rcpwm.hpp

@@ -3,13 +3,16 @@
  * See <https://www.gnu.org/licenses/> for details.
  * Author: Dmitry Ponomarev <ponomarevda96@gmail.com>
  * Author: Anastasiia Stepanova <asiiapine@gmail.com>
+ * Author: Ilia Kliantsevich <iliawork112005@gmail.com>
  */
 
 #include "feedback.hpp"
 #include "peripheral/pwm/pwm.hpp"
 #include "peripheral/adc/circuit_periphery.hpp"
 #include "drivers/rcpwm/rcpwm.hpp"
+#include "modules/rcout/dronecan_frontend/dronecan_frontend.hpp"
 #include "params.hpp"
+#include "logging.hpp"
 
 REGISTER_MODULE(DronecanFeedbackModule)
 
@@ -22,6 +25,7 @@ void DronecanFeedbackModule::update_params() {
     feedback_esc_enabled = static_cast<bool>(paramsGetIntegerValue(IntParamsIndexes::PARAM_FEEDBACK_ESC_ENABLE));
     feedback_actuator_enabled = static_cast<bool>(paramsGetIntegerValue(IntParamsIndexes::PARAM_FEEDBACK_ACTUATOR_ENABLE));
     feedback_hardpoint_enabled = static_cast<bool>(paramsGetIntegerValue(IntParamsIndexes::PARAM_FEEDBACK_HARDPOINT_ENABLE));
+    feedback_gimbal_enabled = static_cast<Gimbal_enable>(paramsGetIntegerValue(IntParamsIndexes::PARAM_FEEDBACK_GIMBAL_ENABLE));
 }
 
 void DronecanFeedbackModule::spin_once() {
@@ -42,11 +46,18 @@ void DronecanFeedbackModule::spin_once() {
             publish_hardpoint_status(pin_idx);
         }
     }
+
+    if (feedback_gimbal_enabled == Gimbal_enable::ENABLE_DEG){
+        publish_gimbal_status_rpy();
+    }
+    if (feedback_gimbal_enabled == Gimbal_enable::ENABLE_QUAT){
+        publish_gimbal_status_quaternion();
+    }
 }
 
 void DronecanFeedbackModule::publish_esc_status(uint8_t pin_idx) {
     esc_status.msg = {
-        .error_count = esc_status.msg.error_count + 1,
+        .error_count = esc_status.msg.error_count + 1, // FIXME: implement proper error counting
         .voltage = CircuitPeriphery::voltage_vin(),
         .current = CircuitPeriphery::current(),
         .temperature = static_cast<float>(CircuitPeriphery::temperature()),
@@ -68,7 +79,6 @@ void DronecanFeedbackModule::publish_actuator_status(uint8_t pin_idx) {
         .force = CircuitPeriphery::current(),
         .speed = static_cast<float>(CircuitPeriphery::temperature()),
 
-        .reserved = 0,
         .power_rating_pct = Driver::RCPWM::get_pin_percent(pin_idx),
     };
 
@@ -89,3 +99,58 @@ void DronecanFeedbackModule::publish_hardpoint_status(uint8_t pin_idx) {
     // Hardpoint serialization has a bug, let's skip it until it is fixed
     // hardpoint_status.publish();
 }
+
+void DronecanFeedbackModule::publish_gimbal_status_rpy() {
+
+    gimbal_status_pub.msg.gimbal_id = 0; //
+    gimbal_status_pub.msg.mode.command_mode = 1;
+    
+    gimbal_status_pub.msg.camera_orientation_in_body_frame_xyzw[0] = 0; // X
+    gimbal_status_pub.msg.camera_orientation_in_body_frame_xyzw[1] = 0; // Y
+    gimbal_status_pub.msg.camera_orientation_in_body_frame_xyzw[2] = 0; // Z
+    gimbal_status_pub.msg.camera_orientation_in_body_frame_xyzw[3] = 1; // W
+    
+    float roll_deg = 0, pitch_deg = 0, yaw_deg = 0;
+    for (size_t i = 0; i < Driver::RCPWM::get_pins_amount(); ++i) {
+        switch (Driver::RCPWM::get_pin_channel(i)) {
+            case 0: // Roll
+                roll_deg = Driver::RCPWM::get_current_angle(gimbal::get_max_servos_angle(), i);
+                break;
+            case 1: // Pitch
+                pitch_deg = Driver::RCPWM::get_current_angle(gimbal::get_max_servos_angle(), i);
+                break;
+            case 2: // Yaw
+                yaw_deg = Driver::RCPWM::get_current_angle(gimbal::get_max_servos_angle(), i);
+                break;
+            default:
+                break;
+        }
+    }
+
+    gimbal_status_pub.msg.camera_orientation_in_body_frame_xyzw[0] = roll_deg; // X
+    gimbal_status_pub.msg.camera_orientation_in_body_frame_xyzw[1] = pitch_deg; // Y
+    gimbal_status_pub.msg.camera_orientation_in_body_frame_xyzw[2] = yaw_deg; // Z
+
+    gimbal_status_pub.publish();
+}
+
+void DronecanFeedbackModule::publish_gimbal_status_quaternion() {
+
+    gimbal_status_pub.msg.gimbal_id = 0;
+    gimbal_status_pub.msg.mode.command_mode = 1;
+    
+    // Set default values for safety
+    gimbal_status_pub.msg.camera_orientation_in_body_frame_xyzw[0] = 0; // X
+    gimbal_status_pub.msg.camera_orientation_in_body_frame_xyzw[1] = 0; // Y
+    gimbal_status_pub.msg.camera_orientation_in_body_frame_xyzw[2] = 0; // Z
+    gimbal_status_pub.msg.camera_orientation_in_body_frame_xyzw[3] = 1; // W
+    
+    const float* q = gimbal::get_quaternion(); 
+
+    gimbal_status_pub.msg.camera_orientation_in_body_frame_xyzw[0] = q[0]; // X
+    gimbal_status_pub.msg.camera_orientation_in_body_frame_xyzw[1] = q[1]; // Y
+    gimbal_status_pub.msg.camera_orientation_in_body_frame_xyzw[2] = q[2]; // Z
+    gimbal_status_pub.msg.camera_orientation_in_body_frame_xyzw[3] = q[3]; // W
+
+    gimbal_status_pub.publish();
+}