AP_Mount: Alexmos: Improve angle precision and move defines to cpp

ArduPilot · Jun 20, 2024 · e1a1d15 · e1a1d15
1 parent 80b1a07
commit e1a1d15
Show file tree

Hide file tree

Showing 2 changed files with 51 additions and 50 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 / float(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: