diff --git a/libraries/AP_Mount/AP_Mount_Alexmos.cpp b/libraries/AP_Mount/AP_Mount_Alexmos.cpp index a08eccf34f..693d197860 100644 --- 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 +//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 index 6485073dd4..a8765e1743 100644 --- a/libraries/AP_Mount/AP_Mount_Alexmos.h +++ b/libraries/AP_Mount/AP_Mount_Alexmos.h @@ -10,56 +10,6 @@ #include #include -//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: