mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
AP_Mount_Alexmos : changes some define to avoid conflicting
change SPEED to AP_MOUNT_ALEXMOS_SPEED and the control mode defines as well
This commit is contained in:
parent
6e5e438b0f
commit
cdeb1fb8f3
@ -130,12 +130,12 @@ void AP_Mount_Alexmos::control_axis(const Vector3f& angle, bool target_in_degree
|
|||||||
target_deg *= RAD_TO_DEG;
|
target_deg *= RAD_TO_DEG;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint8_t mode = MODE_ANGLE;
|
uint8_t mode = AP_MOUNT_ALEXMOS_MODE_ANGLE;
|
||||||
int16_t speed_roll = DEGREE_PER_SEC_TO_VALUE(SPEED);
|
int16_t speed_roll = DEGREE_PER_SEC_TO_VALUE(AP_MOUNT_ALEXMOS_SPEED);
|
||||||
int16_t angle_roll = DEGREE_TO_VALUE(target_deg.x);
|
int16_t angle_roll = DEGREE_TO_VALUE(target_deg.x);
|
||||||
int16_t speed_pitch = DEGREE_PER_SEC_TO_VALUE(SPEED);
|
int16_t speed_pitch = DEGREE_PER_SEC_TO_VALUE(AP_MOUNT_ALEXMOS_SPEED);
|
||||||
int16_t angle_pitch = DEGREE_TO_VALUE(target_deg.y);
|
int16_t angle_pitch = DEGREE_TO_VALUE(target_deg.y);
|
||||||
int16_t speed_yaw = DEGREE_PER_SEC_TO_VALUE(SPEED);
|
int16_t speed_yaw = DEGREE_PER_SEC_TO_VALUE(AP_MOUNT_ALEXMOS_SPEED);
|
||||||
int16_t angle_yaw = DEGREE_TO_VALUE(target_deg.z);
|
int16_t angle_yaw = DEGREE_TO_VALUE(target_deg.z);
|
||||||
uint8_t data[13] = {
|
uint8_t data[13] = {
|
||||||
(uint8_t)mode,
|
(uint8_t)mode,
|
||||||
|
@ -54,13 +54,13 @@
|
|||||||
#define CMD_SERVO_OUT 36
|
#define CMD_SERVO_OUT 36
|
||||||
#define CMD_ERROR 255
|
#define CMD_ERROR 255
|
||||||
|
|
||||||
#define MODE_NO_CONTROL 0
|
#define AP_MOUNT_ALEXMOS_MODE_NO_CONTROL 0
|
||||||
#define MODE_SPEED 1
|
#define AP_MOUNT_ALEXMOS_MODE_SPEED 1
|
||||||
#define MODE_ANGLE 2
|
#define AP_MOUNT_ALEXMOS_MODE_ANGLE 2
|
||||||
#define MODE_SPEED_ANGLE 3
|
#define AP_MOUNT_ALEXMOS_MODE_SPEED_ANGLE 3
|
||||||
#define MODE_RC 4
|
#define AP_MOUNT_ALEXMOS_MODE_RC 4
|
||||||
|
|
||||||
#define SPEED 30 // degree/s2
|
#define AP_MOUNT_ALEXMOS_SPEED 30 // degree/s2
|
||||||
|
|
||||||
#define VALUE_TO_DEGREE(d) ((float)((d * 720) >> 15))
|
#define VALUE_TO_DEGREE(d) ((float)((d * 720) >> 15))
|
||||||
#define DEGREE_TO_VALUE(d) ((int16_t)((float)(d)*(1.0f/0.02197265625f)))
|
#define DEGREE_TO_VALUE(d) ((int16_t)((float)(d)*(1.0f/0.02197265625f)))
|
||||||
|
Loading…
Reference in New Issue
Block a user