Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

ArduCopter: Guided pos_control_run add yaw_rate timeout

Browse Source
This commit is contained in:
Josh Henderson 2021-09-09 00:05:49 -04:00 committed by Peter Barker
parent e9ce1886c8
commit 3642b14a0b
3 changed files with 11 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/Parameters.cpp
View File

@ -1060,7 +1060,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
#if MODE_GUIDED_ENABLED == ENABLED
// @Param: GUID_TIMEOUT
// @DisplayName: Guided mode timeout
// @Description: Guided mode timeout after which vehicle will stop or return to level if no updates are received from caller. Only applicable during velocity, acceleration or angle control
// @Description: Guided mode timeout after which vehicle will stop or return to level if no updates are received from caller. Only applicable during any combination of velocity, acceleration, angle control, and/or angular rate control
// @Units: s
// @Range: 0.1 5
// @User: Advanced

2
ArduCopter/mode.h
View File

@ -923,7 +923,7 @@ public:
void angle_control_start();
void angle_control_run();
// return guided mode timeout in milliseconds. Only used for velocity, acceleration and angle control
// return guided mode timeout in milliseconds. Only used for velocity, acceleration, angle control, and angular rate control
uint32_t get_timeout_ms() const;
bool use_pilot_yaw() const override;

10
ArduCopter/mode_guided.cpp
View File

@ -468,6 +468,7 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y
guided_pos_terrain_alt = terrain_alt;
guided_vel_target_cms.zero();
guided_accel_target_cmss.zero();
update_time_ms = millis();
// log target
copter.Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss);
@ -678,6 +679,13 @@ void ModeGuided::pos_control_run()
guided_accel_target_cmss.zero();
guided_vel_target_cms.zero();
// stop rotating if no updates received within timeout_ms
if (millis() - update_time_ms > get_timeout_ms()) {
if ((auto_yaw.mode() == AUTO_YAW_RATE) || (auto_yaw.mode() == AUTO_YAW_ANGLE_RATE)) {
auto_yaw.set_rate(0.0f);
}
}
float pos_offset_z_buffer = 0.0; // Vertical buffer size in m
if (guided_pos_terrain_alt) {
pos_offset_z_buffer = MIN(copter.wp_nav->get_terrain_margin() * 100.0, 0.5 * fabsf(guided_pos_target_cm.z));
@ -1163,7 +1171,7 @@ float ModeGuided::crosstrack_error() const
return 0;
}
// return guided mode timeout in milliseconds. Only used for velocity, acceleration and angle control
// return guided mode timeout in milliseconds. Only used for velocity, acceleration, angle control, and angular rates
uint32_t ModeGuided::get_timeout_ms() const
{
return MAX(copter.g2.guided_timeout, 0.1) * 1000;