mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-03 12:14:10 -04:00
AP_Mount: Topotek pitch rate direction fix
This commit is contained in:
parent
dc43fa0a76
commit
df24d3a61d
@ -854,7 +854,7 @@ void AP_Mount_Topotek::send_rate_target(const MountTarget& rate_rads)
|
||||
|
||||
// convert and constrain rates
|
||||
const uint8_t roll_angle_speed = constrain_int16(degrees(rate_rads.roll) * ANGULAR_VELOCITY_CONVERSION, -99, 99);
|
||||
const uint8_t pitch_angle_speed = constrain_int16(degrees(rate_rads.pitch) * ANGULAR_VELOCITY_CONVERSION, -99, 99);
|
||||
const uint8_t pitch_angle_speed = constrain_int16(-degrees(rate_rads.pitch) * ANGULAR_VELOCITY_CONVERSION, -99, 99);
|
||||
const uint8_t yaw_angle_speed = constrain_int16(degrees(rate_rads.yaw) * ANGULAR_VELOCITY_CONVERSION, -99, 99);
|
||||
|
||||
// send stop rotation command three times if target roll, pitch and yaw are zero
|
||||
|
Loading…
Reference in New Issue
Block a user