AP_Mount: Topotek pitch rate direction fix

This commit is contained in:
Randy Mackay 2025-01-04 19:52:11 +09:00
parent dc43fa0a76
commit df24d3a61d

View File

@ -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