Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ArduPlane: make Guided update frequency limit configurable

This commit is contained in:
Tim Tuxworth 2024-11-05 11:15:17 +08:00
parent 2892483ea1
commit 6fd68bc311
3 changed files with 13 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
ArduPlane/Parameters.cpp
View File

@ -1288,7 +1288,14 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @User: Standard // @User: Standard
AP_GROUPINFO("RNGFND_LND_ORNT", 36, ParametersG2, rangefinder_land_orient, ROTATION_PITCH_270), AP_GROUPINFO("RNGFND_LND_ORNT", 36, ParametersG2, rangefinder_land_orient, ROTATION_PITCH_270),
#endif #endif
// @Param: GUIDED_UPD_LIM
// @DisplayName: Guided Update Limit
// @Description: The maximum frequency that an guided mode commands sent by external system such as lua or mavlink can update roll, pitch and throttle.
// @Units: ms
// @User: Standard
AP_GROUPINFO("GUIDED_UPD_LIM", 37, ParametersG2, guided_update_frequency_limit, 3000),
AP_GROUPEND AP_GROUPEND
}; };

2
ArduPlane/Parameters.h
View File

@ -566,6 +566,8 @@ public:
AP_Follow follow; AP_Follow follow;
#endif #endif
AP_Int32 guided_update_frequency_limit;
AP_Float fs_ekf_thresh; AP_Float fs_ekf_thresh;
// min initial climb in RTL // min initial climb in RTL

6
ArduPlane/mode_guided.cpp
View File

@ -38,7 +38,7 @@ void ModeGuided::update()
// Received an external msg that guides roll in the last 3 seconds? // Received an external msg that guides roll in the last 3 seconds?
if (plane.guided_state.last_forced_rpy_ms.x > 0 && if (plane.guided_state.last_forced_rpy_ms.x > 0 &&
millis() - plane.guided_state.last_forced_rpy_ms.x < 3000) { (millis() - plane.guided_state.last_forced_rpy_ms.x) < (uint32_t)plane.g2.guided_update_frequency_limit) {
plane.nav_roll_cd = constrain_int32(plane.guided_state.forced_rpy_cd.x, -plane.roll_limit_cd, plane.roll_limit_cd); plane.nav_roll_cd = constrain_int32(plane.guided_state.forced_rpy_cd.x, -plane.roll_limit_cd, plane.roll_limit_cd);
plane.update_load_factor(); plane.update_load_factor();
@ -76,7 +76,7 @@ void ModeGuided::update()
} }
if (plane.guided_state.last_forced_rpy_ms.y > 0 && if (plane.guided_state.last_forced_rpy_ms.y > 0 &&
millis() - plane.guided_state.last_forced_rpy_ms.y < 3000) { (millis() - plane.guided_state.last_forced_rpy_ms.y) < (uint32_t)plane.g2.guided_update_frequency_limit) {
plane.nav_pitch_cd = constrain_int32(plane.guided_state.forced_rpy_cd.y, plane.pitch_limit_min*100, plane.aparm.pitch_limit_max.get()*100); plane.nav_pitch_cd = constrain_int32(plane.guided_state.forced_rpy_cd.y, plane.pitch_limit_min*100, plane.aparm.pitch_limit_max.get()*100);
} else { } else {
plane.calc_nav_pitch(); plane.calc_nav_pitch();
@ -89,7 +89,7 @@ void ModeGuided::update()
} else if (plane.aparm.throttle_cruise > 1 && } else if (plane.aparm.throttle_cruise > 1 &&
plane.guided_state.last_forced_throttle_ms > 0 && plane.guided_state.last_forced_throttle_ms > 0 &&
millis() - plane.guided_state.last_forced_throttle_ms < 3000) { (millis() - plane.guided_state.last_forced_throttle_ms) < (uint32_t)plane.g2.guided_update_frequency_limit) {
// Received an external msg that guides throttle in the last 3 seconds? // Received an external msg that guides throttle in the last 3 seconds?
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.guided_state.forced_throttle); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.guided_state.forced_throttle);