wip

Hwurzburg · Oct 13, 2024 · 3765cf6 · 3765cf6
1 parent 73b8a68
commit 3765cf6
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 4 deletions.
diff --git a/libraries/AP_Mount/AP_Mount_CADDX.cpp b/libraries/AP_Mount/AP_Mount_CADDX.cpp
@@ -134,10 +134,12 @@ void AP_Mount_CADDX::send_target_angles(const MountTarget& angle_target_rad)
     cmd_set_angles_data.pan = degrees(angle_target_rad.get_bf_yaw());
 
     //normalize here
+    cmd_set_angles_data.tilt = int16_t(linear_interpolate(AXIS_MIN, AXIS_MAX, cmd_set_angles_data.tilt, _params.pitch_angle_min, _params.pitch_angle_max));
+    cmd_set_angles_data.roll = int16_t(linear_interpolate(AXIS_MIN, AXIS_MAX, cmd_set_angles_data.roll, _params.roll_angle_min, _params.roll_angle_max));
+    cmd_set_angles_data.pan = int16_t(linear_interpolate(AXIS_MIN, AXIS_MAX, cmd_set_angles_data.pan, _params.yaw_angle_min, _params.yaw_angle_max));
 
-    uint8_t* buf = (uint8_t*)&cmd_set_angles_data;
-
     //todo: add mode here
+    cmd_set_angles_data.mode = GIMBAL_MODE_TILT_ROLL_LOCK;
 
     // CRC here
     uint16_t crc16 = 0;
@@ -149,7 +151,7 @@ void AP_Mount_CADDX::send_target_angles(const MountTarget& angle_target_rad)
     cmd_set_angles_data.crcl = crc16 & 0xFF;
 
     //send to gimbal
-
+    uint8_t* buf = (uint8_t*)&cmd_set_angles_data;    
     for (uint8_t i = 0;  i != sizeof(cmd_set_angles_data) ; i++) {
         _uart->write(buf[i]);
     }

diff --git a/libraries/AP_Mount/AP_Mount_CADDX.h b/libraries/AP_Mount/AP_Mount_CADDX.h
@@ -12,7 +12,20 @@
 #include <AP_Common/AP_Common.h>
 
 #define AP_MOUNT_CADDX_RESEND_MS   1000    // resend angle targets to gimbal once per second
-#define GIMBAL_MODE_DEFAULT 0  //Full follow mode, ie gimbal locked to body frame
+
+#define AXIS_MAX 2047
+#define AXIS_MIN -2048
+
+typedef enum {
+    GIMBAL_MODE_FOLLOW = 0,
+    GIMBAL_MODE_TILT_LOCK = (1<<0),
+    GIMBAL_MODE_ROLL_LOCK = (1<<1),
+    GIMBAL_MODE_PAN_LOCK  = (1<<2),
+} gimbal_lock_mode;
+
+#define GIMBAL_MODE_DEFAULT             GIMBAL_MODE_FOLLOW
+#define GIMBAL_MODE_TILT_ROLL_LOCK      (GIMBAL_MODE_TILT_LOCK | GIMBAL_MODE_ROLL_LOCK)
+#define GIMBAL_MODE_PAN_TILT_ROLL_LOCK  (GIMBAL_MODE_TILT_LOCK | GIMBAL_MODE_ROLL_LOCK | GIMBAL_MODE_PAN_LOCK)
 
 class AP_Mount_CADDX : public AP_Mount_Backend_Serial
 {