AP_Mount: Alexmos: Cleanup defines, remove unused variables, and incr…

…ease precision of VALUE_TO_DEGREE
MattKear · Jun 14, 2024 · 8552b47 · 8552b47
1 parent ee14ff3
commit 8552b47
Show file tree

Hide file tree

Showing 2 changed files with 54 additions and 59 deletions.
diff --git a/libraries/AP_Mount/AP_Mount_Alexmos.cpp b/libraries/AP_Mount/AP_Mount_Alexmos.cpp
@@ -3,6 +3,57 @@
 #if HAL_MOUNT_ALEXMOS_ENABLED
 #include <AP_SerialManager/AP_SerialManager.h>
 
+//definition of the commands id for the Alexmos Serial Protocol
+#define CMD_READ_PARAMS 'R'
+#define CMD_WRITE_PARAMS 'W'
+#define CMD_REALTIME_DATA 'D'
+#define CMD_BOARD_INFO 'V'
+#define CMD_CALIB_ACC 'A'
+#define CMD_CALIB_GYRO 'g'
+#define CMD_CALIB_EXT_GAIN 'G'
+#define CMD_USE_DEFAULTS 'F'
+#define CMD_CALIB_POLES 'P'
+#define CMD_RESET 'r'
+#define CMD_HELPER_DATA 'H'
+#define CMD_CALIB_OFFSET 'O'
+#define CMD_CALIB_BAT 'B'
+#define CMD_MOTORS_ON 'M'
+#define CMD_MOTORS_OFF 'm'
+#define CMD_CONTROL 'C'
+#define CMD_TRIGGER_PIN 'T'
+#define CMD_EXECUTE_MENU 'E'
+#define CMD_GET_ANGLES 'I'
+#define CMD_CONFIRM 'C'
+// Board v3.x only
+#define CMD_BOARD_INFO_3 20
+#define CMD_READ_PARAMS_3 21
+#define CMD_WRITE_PARAMS_3 22
+#define CMD_REALTIME_DATA_3 23
+#define CMD_SELECT_IMU_3 24
+#define CMD_READ_PROFILE_NAMES 28
+#define CMD_WRITE_PROFILE_NAMES 29
+#define CMD_QUEUE_PARAMS_INFO_3 30
+#define CMD_SET_PARAMS_3 31
+#define CMD_SAVE_PARAMS_3 32
+#define CMD_READ_PARAMS_EXT 33
+#define CMD_WRITE_PARAMS_EXT 34
+#define CMD_AUTO_PID 35
+#define CMD_SERVO_OUT 36
+#define CMD_ERROR 255
+
+#define AP_MOUNT_ALEXMOS_MODE_NO_CONTROL 0
+#define AP_MOUNT_ALEXMOS_MODE_SPEED 1
+#define AP_MOUNT_ALEXMOS_MODE_ANGLE 2
+#define AP_MOUNT_ALEXMOS_MODE_SPEED_ANGLE 3
+#define AP_MOUNT_ALEXMOS_MODE_RC 4
+
+#define AP_MOUNT_ALEXMOS_SPEED 30 // deg/s
+
+#define INT14_DEGREES = (360.0f / 0x3FFF) // 1 full rotation in degrees over 14 bit range
+#define VALUE_TO_DEGREE(d) (float(d)*INT14_DEGREES)
+#define DEGREE_TO_VALUE(d) ((int16_t)((float)(d)*(1.0f/INT14_DEGREES)))
+#define DEGREE_PER_SEC_TO_VALUE(d) ((int16_t)((float)(d)*(1.0f/0.1220740379f)))
+
 void AP_Mount_Alexmos::init()
 {
     const AP_SerialManager& serial_manager = AP::serialmanager();

diff --git a/libraries/AP_Mount/AP_Mount_Alexmos.h b/libraries/AP_Mount/AP_Mount_Alexmos.h
@@ -10,56 +10,6 @@
 #include <AP_Param/AP_Param.h>
 #include <AP_Math/AP_Math.h>
 
-//definition of the commands id for the Alexmos Serial Protocol
-#define CMD_READ_PARAMS 'R'
-#define CMD_WRITE_PARAMS 'W'
-#define CMD_REALTIME_DATA 'D'
-#define CMD_BOARD_INFO 'V'
-#define CMD_CALIB_ACC 'A'
-#define CMD_CALIB_GYRO 'g'
-#define CMD_CALIB_EXT_GAIN 'G'
-#define CMD_USE_DEFAULTS 'F'
-#define CMD_CALIB_POLES 'P'
-#define CMD_RESET 'r'
-#define CMD_HELPER_DATA 'H'
-#define CMD_CALIB_OFFSET 'O'
-#define CMD_CALIB_BAT 'B'
-#define CMD_MOTORS_ON 'M'
-#define CMD_MOTORS_OFF 'm'
-#define CMD_CONTROL 'C'
-#define CMD_TRIGGER_PIN 'T'
-#define CMD_EXECUTE_MENU 'E'
-#define CMD_GET_ANGLES 'I'
-#define CMD_CONFIRM 'C'
-// Board v3.x only
-#define CMD_BOARD_INFO_3 20
-#define CMD_READ_PARAMS_3 21
-#define CMD_WRITE_PARAMS_3 22
-#define CMD_REALTIME_DATA_3 23
-#define CMD_SELECT_IMU_3 24
-#define CMD_READ_PROFILE_NAMES 28
-#define CMD_WRITE_PROFILE_NAMES 29
-#define CMD_QUEUE_PARAMS_INFO_3 30
-#define CMD_SET_PARAMS_3 31
-#define CMD_SAVE_PARAMS_3 32
-#define CMD_READ_PARAMS_EXT 33
-#define CMD_WRITE_PARAMS_EXT 34
-#define CMD_AUTO_PID 35
-#define CMD_SERVO_OUT 36
-#define CMD_ERROR 255
-
-#define AP_MOUNT_ALEXMOS_MODE_NO_CONTROL 0
-#define AP_MOUNT_ALEXMOS_MODE_SPEED 1
-#define AP_MOUNT_ALEXMOS_MODE_ANGLE 2
-#define AP_MOUNT_ALEXMOS_MODE_SPEED_ANGLE 3
-#define AP_MOUNT_ALEXMOS_MODE_RC 4
-
-#define AP_MOUNT_ALEXMOS_SPEED 30 // deg/s
-
-#define VALUE_TO_DEGREE(d) ((float)((d * 720) >> 15))
-#define DEGREE_TO_VALUE(d) ((int16_t)((float)(d)*(1.0f/0.02197265625f)))
-#define DEGREE_PER_SEC_TO_VALUE(d) ((int16_t)((float)(d)*(1.0f/0.1220740379f)))
-
 class AP_Mount_Alexmos : public AP_Mount_Backend
 {
 public:
@@ -124,15 +74,9 @@ class AP_Mount_Alexmos : public AP_Mount_Backend
 
     // CMD_GET_ANGLES
     struct PACKED alexmos_angles {
-        int16_t angle_roll;
-        int16_t rc_angle_roll;
-        int16_t rc_speed_roll;
-        int16_t angle_pitch;
-        int16_t rc_angle_pitch;
-        int16_t rc_speed_pitch;
-        int16_t angle_yaw;
-        int16_t rc_angle_yaw;
-        int16_t rc_speed_yaw;
+        float angle_roll;
+        float angle_pitch;
+        float angle_yaw;
     };
 
     // CMD_CONTROL