Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-06 19:00:27 +08:00
Code Issues Packages Projects Releases Wiki Activity

Copter: Convert Mode FlowHold to meters and radians

Browse Source
This commit is contained in:
Leonard Hall
2025-08-20 17:01:44 +09:00
committed by Randy Mackay
parent d56e91bbe2
commit f2e9026aa2
1 changed files with 35 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ArduCopter/mode_flowhold.cpp
+35 -36
View File
@@ -115,34 +115,35 @@ bool ModeFlowHold::init(bool ignore_checks)
/*
calculate desired attitude from flow sensor. Called when flow sensor is healthy
*/
void ModeFlowHold::flowhold_flow_to_angle(Vector2f &bf_angles_cd, bool stick_input)
void ModeFlowHold::flowhold_flow_to_angle(Vector2f &bf_angles_rad, bool stick_input)
{
uint32_t now = AP_HAL::millis();
const float angle_max_rad = cd_to_rad(copter.aparm.angle_max);
// get corrected raw flow rate
Vector2f raw_flow = copter.optflow.flowRate() - copter.optflow.bodyRate();
Vector2f raw_flow_rads = copter.optflow.flowRate() - copter.optflow.bodyRate();
// limit sensor flow, this prevents oscillation at low altitudes
raw_flow.x = constrain_float(raw_flow.x, -flow_max, flow_max);
raw_flow.y = constrain_float(raw_flow.y, -flow_max, flow_max);
raw_flow_rads.x = constrain_float(raw_flow_rads.x, -flow_max, flow_max);
raw_flow_rads.y = constrain_float(raw_flow_rads.y, -flow_max, flow_max);
// filter the flow rate
Vector2f sensor_flow = flow_filter.apply(raw_flow);
Vector2f sensor_flow_rads = flow_filter.apply(raw_flow_rads);
// scale by height estimate, limiting it to height_min_m to height_max
float ins_height_m = pos_control->get_pos_estimate_NEU_m().z;
float height_estimate_m = ins_height_m + height_offset_m;
// compensate for height, this converts to (approx) m/s
sensor_flow *= constrain_float(height_estimate_m, height_min_m, height_max);
Vector2f sensor_flow_ms = sensor_flow_rads * constrain_float(height_estimate_m, height_min_m, height_max);
// rotate controller input to earth frame
Vector2f input_ef = copter.ahrs.body_to_earth2D(sensor_flow);
Vector2f input_ne_ms = copter.ahrs.body_to_earth2D(sensor_flow_ms);
// run PI controller
flow_pi_xy.set_input(input_ef);
flow_pi_xy.set_input(input_ne_ms);
// get earth frame controller attitude in centi-degrees
// get the normalised earth frame controller attitude
Vector2f ef_output;
// get P term
@@ -154,11 +155,11 @@ void ModeFlowHold::flowhold_flow_to_angle(Vector2f &bf_angles_cd, bool stick_inp
}
if (!stick_input && braking) {
// stop braking if either 3s has passed, or we have slowed below 0.3m/s
if (now - last_stick_input_ms > 3000 || sensor_flow.length() < 0.3) {
if (now - last_stick_input_ms > 3000 || sensor_flow_ms.length() < 0.3) {
braking = false;
#if 0
printf("braking done at %u vel=%f\n", now - last_stick_input_ms,
(double)sensor_flow.length());
(double)sensor_flow_ms.length());
#endif
}
}
@@ -177,30 +178,30 @@ void ModeFlowHold::flowhold_flow_to_angle(Vector2f &bf_angles_cd, bool stick_inp
if (!stick_input && braking) {
// calculate brake angle for each axis separately
for (uint8_t i=0; i<2; i++) {
float &velocity = sensor_flow[i];
float abs_vel_cms = fabsf(velocity)*100;
float &velocity = sensor_flow_ms[i];
float abs_vel_ms = fabsf(velocity);
const float brake_gain = (15.0f * brake_rate_dps.get() + 95.0f) * 0.01f;
float lean_angle_cd = brake_gain * abs_vel_cms * (1.0f+500.0f/(abs_vel_cms+60.0f));
float lean_angle_rad = radians(1.0f) * brake_gain * abs_vel_ms * (1.0f + 5.0f/(abs_vel_ms + 0.60f));
if (velocity < 0) {
lean_angle_cd = -lean_angle_cd;
lean_angle_rad = -lean_angle_rad;
}
bf_angles_cd[i] = lean_angle_cd;
bf_angles_rad[i] = lean_angle_rad;
}
ef_output.zero();
}
ef_output += xy_I;
ef_output *= copter.aparm.angle_max;
Vector2f ef_output_rad = ef_output * cd_to_rad(copter.aparm.angle_max);
// convert to body frame
bf_angles_cd += copter.ahrs.earth_to_body2D(ef_output);
bf_angles_rad += copter.ahrs.earth_to_body2D(ef_output_rad);
// set limited flag to prevent integrator windup
limited = fabsf(bf_angles_cd.x) > copter.aparm.angle_max || fabsf(bf_angles_cd.y) > copter.aparm.angle_max;
limited = fabsf(bf_angles_rad.x) > angle_max_rad || fabsf(bf_angles_rad.y) > angle_max_rad;
// constrain to angle limit
bf_angles_cd.x = constrain_float(bf_angles_cd.x, -copter.aparm.angle_max, copter.aparm.angle_max);
bf_angles_cd.y = constrain_float(bf_angles_cd.y, -copter.aparm.angle_max, copter.aparm.angle_max);
bf_angles_rad.x = constrain_float(bf_angles_rad.x, -angle_max_rad, angle_max_rad);
bf_angles_rad.y = constrain_float(bf_angles_rad.y, -angle_max_rad, angle_max_rad);
#if HAL_LOGGING_ENABLED
// @LoggerMessage: FHLD
@@ -218,8 +219,8 @@ void ModeFlowHold::flowhold_flow_to_angle(Vector2f &bf_angles_cd, bool stick_inp
if (log_counter++ % 20 == 0) {
AP::logger().WriteStreaming("FHLD", "TimeUS,SFx,SFy,Ax,Ay,Qual,Ix,Iy", "Qfffffff",
AP_HAL::micros64(),
(double)sensor_flow.x, (double)sensor_flow.y,
(double)bf_angles_cd.x, (double)bf_angles_cd.y,
(double)sensor_flow_ms.x, (double)sensor_flow_ms.y,
(double)rad_to_cd(bf_angles_rad.x), (double)rad_to_cd(bf_angles_rad.y),
(double)quality_filtered,
(double)xy_I.x, (double)xy_I.y);
}
@@ -248,7 +249,7 @@ void ModeFlowHold::run()
target_climb_rate_ms = constrain_float(target_climb_rate_ms, -get_pilot_speed_dn_ms(), get_pilot_speed_up_ms());
// get pilot's desired yaw rate
float target_yaw_rate_cds = rad_to_cd(get_pilot_desired_yaw_rate_rads());
float target_yaw_rate_rads = get_pilot_desired_yaw_rate_rads();
// Flow Hold State Machine Determination
AltHoldModeState flowhold_state = get_alt_hold_state_U_ms(target_climb_rate_ms);
@@ -313,17 +314,17 @@ void ModeFlowHold::run()
}
// flowhold attitude target calculations
Vector2f bf_angles_cd;
Vector2f bf_angles_rad;
// calculate alt-hold angles
int16_t roll_in = copter.channel_roll->get_control_in();
int16_t pitch_in = copter.channel_pitch->get_control_in();
float angle_max_cd = copter.aparm.angle_max;
const float angle_max_rad = cd_to_rad(copter.aparm.angle_max);
float target_roll_rad, target_pitch_rad;
get_pilot_desired_lean_angles_rad(target_roll_rad, target_pitch_rad, attitude_control->lean_angle_max_rad(), attitude_control->get_althold_lean_angle_max_rad());
bf_angles_cd.x = rad_to_cd(target_roll_rad);
bf_angles_cd.y = rad_to_cd(target_pitch_rad);
bf_angles_rad.x = target_roll_rad;
bf_angles_rad.y = target_pitch_rad;
if (quality_filtered >= flow_min_quality &&
AP_HAL::millis() - copter.arm_time_ms > 3000) {
@@ -331,22 +332,20 @@ void ModeFlowHold::run()
Vector2f flow_angles;
flowhold_flow_to_angle(flow_angles, (roll_in != 0) || (pitch_in != 0));
flow_angles.x = constrain_float(flow_angles.x, -angle_max_cd/2, angle_max_cd/2);
flow_angles.y = constrain_float(flow_angles.y, -angle_max_cd/2, angle_max_cd/2);
bf_angles_cd += flow_angles;
flow_angles.x = constrain_float(flow_angles.x, -angle_max_rad/2, angle_max_rad/2);
flow_angles.y = constrain_float(flow_angles.y, -angle_max_rad/2, angle_max_rad/2);
bf_angles_rad += flow_angles;
}
bf_angles_cd.x = constrain_float(bf_angles_cd.x, -angle_max_cd, angle_max_cd);
bf_angles_cd.y = constrain_float(bf_angles_cd.y, -angle_max_cd, angle_max_cd);
bf_angles_rad.x = constrain_float(bf_angles_rad.x, -angle_max_rad, angle_max_rad);
bf_angles_rad.y = constrain_float(bf_angles_rad.y, -angle_max_rad, angle_max_rad);
#if AP_AVOIDANCE_ENABLED
// apply avoidance
Vector2f bf_angles_rad = bf_angles_cd * cd_to_rad(1.0);
copter.avoid.adjust_roll_pitch_rad(bf_angles_rad.x, bf_angles_rad.y, attitude_control->lean_angle_max_rad());
bf_angles_cd = bf_angles_rad * rad_to_cd(1.0);
#endif
// call attitude controller
copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_rad(cd_to_rad(bf_angles_cd.x), cd_to_rad(bf_angles_cd.y), cd_to_rad(target_yaw_rate_cds));
copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_rad(bf_angles_rad.x, bf_angles_rad.y, target_yaw_rate_rads);
// run the vertical position controller and set output throttle
pos_control->update_U_controller();