diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index 06316268ad2d68..17c57f76c16fe5 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.cpp +++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp @@ -6,6 +6,7 @@ #include #include #include +#include extern const AP_HAL::HAL& hal; @@ -13,9 +14,15 @@ extern const AP_HAL::HAL& hal; #define AP_MOUNT_SIYI_HEADER2 0x66 // second header byte #define AP_MOUNT_SIYI_PACKETLEN_MIN 10 // minimum number of bytes in a packet. this is a packet with no data bytes #define AP_MOUNT_SIYI_DATALEN_MAX (AP_MOUNT_SIYI_PACKETLEN_MAX-AP_MOUNT_SIYI_PACKETLEN_MIN) // max bytes for data portion of packet -#define AP_MOUNT_SIYI_RATE_MAX_RADS radians(90) // maximum physical rotation rate of gimbal in radans/sec -#define AP_MOUNT_SIYI_PITCH_P 1.50 // pitch controller P gain (converts pitch angle error to target rate) -#define AP_MOUNT_SIYI_YAW_P 1.50 // yaw controller P gain (converts yaw angle error to target rate) +#define AP_MOUNT_SIYI_RATE_MAX_RADS radians(70) // maximum physical rotation rate of gimbal in radans/sec +#define AP_MOUNT_SIYI_ACCEL_MAX_RADSS radians(100) // maximum acceleration of gimbal in rad/s/s +#define AP_MOUNT_SIYI_UPDATE_DT 0.02 // update called at 50hz +#define AP_MOUNT_SIYI_PITCH_P 1.5 // pitch controller P gain (converts pitch angle error to target rate) +#define AP_MOUNT_SIYI_PITCH_I 0.5 // pitch controller I gain (converts persistent pitch angle error to target rate) +#define AP_MOUNT_SIYI_PITCH_IMAX 0.3 // pitch controller IMAX (limits maximum I output) +#define AP_MOUNT_SIYI_YAW_P 1.5 // yaw controller P gain (converts yaw angle error to target rate) +#define AP_MOUNT_SIYI_YAW_I 0.5 // yaw controller I gain (converts persistent yaw angle error to target rate) +#define AP_MOUNT_SIYI_YAW_IMAX 0.3 // yaw controller IMAX (limits maximum I output) #define AP_MOUNT_SIYI_TIMEOUT_MS 1000 // timeout for health and rangefinder readings #define AP_MOUNT_SIYI_DEBUG 0 @@ -32,6 +39,26 @@ const AP_Mount_Siyi::HWInfo AP_Mount_Siyi::hardware_lookup_table[] { {{'7','A'}, "ZT30"}, }; +// rate mapping from radians/sec to scalars that can be sent to the gimbal (e.g. -100 to +100) +const AP_Mount_Siyi::RateMapping AP_Mount_Siyi::rate_mapping[] { + {radians(0), 0.0}, + {radians(4), 0.0}, + {radians(5), 1.75}, + {radians(6), 3.0}, + {radians(7), 4.7}, + {radians(8), 6.0}, + {radians(9), 7.5}, + {radians(10), 9.0}, + {radians(15), 16.2}, + {radians(20), 23.8}, + {radians(25), 31.2}, + {radians(30), 38.6}, + {radians(40), 53.4}, + {radians(50), 68.1}, + {radians(60), 83.4}, + {radians(70), 98.0} +}; + // update mount position - should be called periodically void AP_Mount_Siyi::update() { @@ -670,13 +697,92 @@ bool AP_Mount_Siyi::set_motion_mode(const GimbalMotionMode mode, const bool forc return sent; } +// convert a rate in radians/sec to a scalar that can be sent to the gimbal (e.g. -100 to +100) +float AP_Mount_Siyi::get_rate_scalar(float rate_rads) const +{ + // use absolute rate + float rate_rads_abs = fabsf(rate_rads); + + // find first mapping array element with a desired rate greater than rate_rads_abs + uint8_t high_index = 0; + while (high_index < ARRAY_SIZE(rate_mapping) && (rate_rads_abs > rate_mapping[high_index].desired_rate_rads)) { + high_index++; + } + + // check for out of range + if (high_index == 0) { + return 0.0; + } + + // linear interpolate to find rate scalar + const float dir = rate_rads >= 0 ? 1.0 : -1.0; + const float rate_scalar = dir * linear_interpolate(rate_mapping[high_index-1].rate_scalar, + rate_mapping[high_index].rate_scalar, + rate_rads_abs, + rate_mapping[high_index-1].desired_rate_rads, + rate_mapping[high_index].desired_rate_rads); + return rate_scalar; +} + // send target pitch and yaw rates to gimbal // yaw_is_ef should be true if yaw_rads target is an earth frame rate, false if body_frame void AP_Mount_Siyi::send_target_rates(float pitch_rads, float yaw_rads, bool yaw_is_ef) { - const float pitch_rate_scalar = constrain_float(100.0 * pitch_rads / AP_MOUNT_SIYI_RATE_MAX_RADS, -100, 100); - const float yaw_rate_scalar = constrain_float(100.0 * yaw_rads / AP_MOUNT_SIYI_RATE_MAX_RADS, -100, 100); - rotate_gimbal(pitch_rate_scalar, yaw_rate_scalar, yaw_is_ef); + // calculate maximum rate change + const float rate_change_max = AP_MOUNT_SIYI_ACCEL_MAX_RADSS * AP_MOUNT_SIYI_UPDATE_DT; + + // apply limits to pitch rate + _pitch_control.rate_limited = (pitch_rads > _pitch_control.rate_prev + rate_change_max) || (pitch_rads < _pitch_control.rate_prev - rate_change_max) || (fabsf(pitch_rads) > AP_MOUNT_SIYI_RATE_MAX_RADS); + pitch_rads = constrain_float(pitch_rads, _pitch_control.rate_prev - rate_change_max, _pitch_control.rate_prev + rate_change_max); + pitch_rads = constrain_float(pitch_rads, -AP_MOUNT_SIYI_RATE_MAX_RADS, AP_MOUNT_SIYI_RATE_MAX_RADS); + _pitch_control.rate_prev = pitch_rads; + + // apply limits to yaw rate + _yaw_control.rate_limited = (yaw_rads > _yaw_control.rate_prev + rate_change_max) || (yaw_rads < _yaw_control.rate_prev - rate_change_max) || (fabsf(yaw_rads) > AP_MOUNT_SIYI_RATE_MAX_RADS); + yaw_rads = constrain_float(yaw_rads, _yaw_control.rate_prev - rate_change_max, _yaw_control.rate_prev + rate_change_max); + yaw_rads = constrain_float(yaw_rads, -AP_MOUNT_SIYI_RATE_MAX_RADS, AP_MOUNT_SIYI_RATE_MAX_RADS); + _yaw_control.rate_prev = yaw_rads; + + rotate_gimbal(get_rate_scalar(pitch_rads), get_rate_scalar(yaw_rads), yaw_is_ef); + +#if AP_MOUNT_SIYI_DEBUG + // calculate actual pitch and yaw rates + static Vector3f prev_angle_rad; + static uint32_t prev_angle_rad_ms; + float dt = (_last_current_angle_rad_ms - prev_angle_rad_ms) / 1000.0; + if (!is_positive(dt)) { + return; + } + const Vector3f actual_rate = (_current_angle_rad - prev_angle_rad) / dt; + prev_angle_rad = _current_angle_rad; + prev_angle_rad_ms = _last_current_angle_rad_ms; + + // @Description: Siyi Rate Debug + // @Field: TimeUS: Time since system startup + // @Field: DPR: Desired pitch rate + // @Field: PR: Actual pitch rate + // @Field: POut: Pitch controller output + // @Field: PL: Pitch controller rate limited + // @Field: DYaw: Desired yaw angle + // @Field: Yaw: Actual yaw angle + // @Field: YOut: Yaw controller output + // @Field: YL: Yaw controller rate limited + AP::logger().WriteStreaming( + "SIYR", + "TimeUS,DPR,PR,POut,PL,DYR,YR,YOut,YL", + "sdd--dd--", + "F00000000", + "Qfffbfffb", + AP_HAL::micros64(), + (double)degrees(pitch_rads), + (double)degrees(actual_rate.y), + (double)get_rate_scalar(pitch_rads), + (int)_pitch_control.rate_limited, + (double)degrees(yaw_rads), + (double)degrees(actual_rate.z), + (double)get_rate_scalar(yaw_rads), + (int)_yaw_control.rate_limited); +#endif // AP_MOUNT_SIYI_DEBUG } // send target pitch and yaw angles to gimbal @@ -686,7 +792,9 @@ void AP_Mount_Siyi::send_target_angles(float pitch_rad, float yaw_rad, bool yaw_ // stop gimbal if no recent actual angles uint32_t now_ms = AP_HAL::millis(); if (now_ms - _last_current_angle_rad_ms >= 200) { - rotate_gimbal(0, 0, false); + send_target_rates(0, 0, false); + _pitch_control.integrator = 0; + _yaw_control.integrator = 0; return; } @@ -697,9 +805,14 @@ void AP_Mount_Siyi::send_target_angles(float pitch_rad, float yaw_rad, bool yaw_ current_angle_transformed.z = -_current_angle_rad.z; } - // use simple P controller to convert pitch angle error (in radians) to a target rate scalar (-100 to +100) - const float pitch_err_rad = (pitch_rad - current_angle_transformed.y); - const float pitch_rate_scalar = constrain_float(100.0 * pitch_err_rad * AP_MOUNT_SIYI_PITCH_P / AP_MOUNT_SIYI_RATE_MAX_RADS, -100, 100); + // use PI controller to convert pitch angle error (in radians) to a target rate + const float pitch_err_rad = pitch_rad - current_angle_transformed.y; + const float pitch_control_P = pitch_err_rad * AP_MOUNT_SIYI_PITCH_P; + // calculate I term if not limited or error reduces I term + if (!_pitch_control.rate_limited || (pitch_err_rad > 0 && _pitch_control.integrator < 0) || (pitch_err_rad < 0 && _pitch_control.integrator > 0)) { + _pitch_control.integrator = constrain_float(_pitch_control.integrator + pitch_err_rad * AP_MOUNT_SIYI_PITCH_I * AP_MOUNT_SIYI_UPDATE_DT, -AP_MOUNT_SIYI_PITCH_IMAX, AP_MOUNT_SIYI_PITCH_IMAX); + } + const float pitch_rate_rads = pitch_control_P + _pitch_control.integrator; // convert yaw angle to body-frame float yaw_bf_rad = yaw_is_ef ? wrap_PI(yaw_rad - AP::ahrs().get_yaw()) : yaw_rad; @@ -712,12 +825,59 @@ void AP_Mount_Siyi::send_target_angles(float pitch_rad, float yaw_rad, bool yaw_ yaw_is_ef = false; } - // use simple P controller to convert yaw angle error to a target rate scalar (-100 to +100) - const float yaw_err_rad = (yaw_bf_rad - current_angle_transformed.z); - const float yaw_rate_scalar = constrain_float(100.0 * yaw_err_rad * AP_MOUNT_SIYI_YAW_P / AP_MOUNT_SIYI_RATE_MAX_RADS, -100, 100); + // use PI controller to convert yaw angle error (in radians) to a target rate + const float yaw_err_rad = yaw_bf_rad - current_angle_transformed.z; + const float yaw_control_P = yaw_err_rad * AP_MOUNT_SIYI_YAW_P; + // calculate I term if not limited or error reduces I term + if (!_yaw_control.rate_limited || (yaw_err_rad > 0 && _yaw_control.integrator < 0) || (yaw_err_rad < 0 && _yaw_control.integrator > 0)) { + _yaw_control.integrator = constrain_float(_yaw_control.integrator + yaw_err_rad * AP_MOUNT_SIYI_YAW_I * AP_MOUNT_SIYI_UPDATE_DT, -AP_MOUNT_SIYI_YAW_IMAX, AP_MOUNT_SIYI_YAW_IMAX); + } + const float yaw_rate_rads = yaw_control_P + _yaw_control.integrator; - // rotate gimbal. pitch_rate and yaw_rate are scalars in the range -100 ~ +100 - rotate_gimbal(pitch_rate_scalar, yaw_rate_scalar, yaw_is_ef); + // send target rates to gimbal + send_target_rates(pitch_rate_rads, yaw_rate_rads, yaw_is_ef); + +#if AP_MOUNT_SIYI_DEBUG + const float pitch_rate_scalar = get_rate_scalar(pitch_rate_rads); + const float yaw_rate_scalar = get_rate_scalar(yaw_rate_rads); + // @Description: Siyi Pitch Yaw Control + // @Field: TimeUS: Time since system startup + // @Field: DPit: Desired pitch angle + // @Field: Pit: Actual pitch angle + // @Field: PErr: Pitch angle error + // @Field: PP: Pitch controller P output + // @Field: PI: Pitch controller I output + // @Field: POut: Pitch controller output + // @Field: PL: Pitch controller rate limited + // @Field: DYaw: Desired yaw angle + // @Field: Yaw: Actual yaw angle + // @Field: YErr: Yaw angle error + // @Field: YP: Yaw controller P output + // @Field: YI: Yaw controller I output + // @Field: YOut: Yaw controller output + // @Field: YL: Yaw controller rate limited + AP::logger().WriteStreaming( + "SIYI", + "TimeUS,DPit,Pit,PErr,PP,PI,POut,PL,DYaw,Yaw,YErr,YP,YI,YOut,YL", + "sddd----ddd----", + "F00000000000000", + "Qffffffbffffffb", + AP_HAL::micros64(), + (double)degrees(pitch_rad), + (double)degrees(current_angle_transformed.y), + (double)degrees(pitch_err_rad), + (double)pitch_control_P, + (double)_pitch_control.integrator, + (double)pitch_rate_scalar, + (int)_pitch_control.rate_limited, + (double)degrees(yaw_bf_rad), + (double)degrees(current_angle_transformed.z), + (double)degrees(yaw_err_rad), + (double)yaw_control_P, + (double)_yaw_control.integrator, + (double)yaw_rate_scalar, + (int)_yaw_control.rate_limited); +#endif // AP_MOUNT_SIYI_DEBUG } // take a picture. returns true on success diff --git a/libraries/AP_Mount/AP_Mount_Siyi.h b/libraries/AP_Mount/AP_Mount_Siyi.h index 957c9f8c442bb7..f58da1f627ff77 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.h +++ b/libraries/AP_Mount/AP_Mount_Siyi.h @@ -253,6 +253,9 @@ class AP_Mount_Siyi : public AP_Mount_Backend_Serial void request_gimbal_attitude() { send_packet(SiyiCommandId::ACQUIRE_GIMBAL_ATTITUDE, nullptr, 0); } void request_rangefinder_distance() { send_packet(SiyiCommandId::READ_RANGEFINDER, nullptr, 0); } + // convert a rate in radians/sec to a scalar that can be sent to the gimbal (e.g. -100 to +100) + float get_rate_scalar(float rate_rads) const; + // rotate gimbal. pitch_rate and yaw_rate are scalars in the range -100 ~ +100 // yaw_is_ef should be true if gimbal should maintain an earth-frame target (aka lock) void rotate_gimbal(int8_t pitch_scalar, int8_t yaw_scalar, bool yaw_is_ef); @@ -344,6 +347,20 @@ class AP_Mount_Siyi : public AP_Mount_Backend_Serial }; static const HWInfo hardware_lookup_table[]; + // rate mapping from radians/sec to scalars that can be sent to the gimbal (e.g. -100 to +100) + struct RateMapping { + float desired_rate_rads; + float rate_scalar; + }; + static const RateMapping rate_mapping[]; + + // pitch and yaw controller + struct RateControl { + float integrator; // rate control integrator + float rate_prev; // previous target rate (used for acceleration limiting) + bool rate_limited; // true if rate was acceleration limited + } _pitch_control, _yaw_control; + // count of SET_TIME packets, we send 5 times to cope with packet loss uint8_t sent_time_count; };