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

ArduPlane: changes to support scripted plane follow

This commit is contained in:
Tim Tuxworth 2024-11-05 11:15:17 +08:00
parent b9352f0644
commit 8910d83f4e
4 changed files with 16 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduPlane/Parameters.cpp
View File

@ -1304,6 +1304,13 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
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

@ -571,6 +571,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

2
ArduPlane/commands.cpp
View File

@ -87,7 +87,9 @@ void Plane::set_guided_WP(const Location &loc)
// used to control FBW and limit the rate of climb // used to control FBW and limit the rate of climb
// ----------------------------------------------- // -----------------------------------------------
if (!control_mode->is_guided_mode()) {
set_target_altitude_current(); set_target_altitude_current();
}
setup_glide_slope(); setup_glide_slope();
setup_turn_angle(); setup_turn_angle();

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);