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

Copter: add GUID_TIMEOUT for guided mode vel, accel and angle control

This commit is contained in:
Randy Mackay 2021-07-09 14:02:07 +09:00
parent 1aa3ef9b67
commit 50d0592962
4 changed files with 27 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduCopter/Parameters.cpp
View File

@ -1069,6 +1069,16 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
AP_GROUPINFO("RNGFND_FILT", 45, ParametersG2, rangefinder_filt, RANGEFINDER_FILT_DEFAULT), AP_GROUPINFO("RNGFND_FILT", 45, ParametersG2, rangefinder_filt, RANGEFINDER_FILT_DEFAULT),
#endif #endif
#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
// @Units: s
// @Range: 0.1 5
// @User: Advanced
AP_GROUPINFO("GUID_TIMEOUT", 46, ParametersG2, guided_timeout, 3.0),
#endif
AP_GROUPEND AP_GROUPEND
}; };

4
ArduCopter/Parameters.h
View File

@ -649,6 +649,10 @@ public:
AP_Float rangefinder_filt; AP_Float rangefinder_filt;
#endif #endif
#if MODE_GUIDED_ENABLED == ENABLED
AP_Float guided_timeout;
#endif
}; };
extern const AP_Param::Info var_info[]; extern const AP_Param::Info var_info[];

3
ArduCopter/mode.h
View File

@ -890,6 +890,9 @@ public:
void angle_control_start(); void angle_control_start();
void angle_control_run(); void angle_control_run();
// return guided mode timeout in milliseconds. Only used for velocity, acceleration and angle control
uint32_t get_timeout_ms() const;
protected: protected:
const char *name() const override { return "GUIDED"; } const char *name() const override { return "GUIDED"; }

17
ArduCopter/mode_guided.cpp
View File

@ -10,9 +10,6 @@
# define GUIDED_LOOK_AT_TARGET_MIN_DISTANCE_CM 500 // point nose at target if it is more than 5m away # define GUIDED_LOOK_AT_TARGET_MIN_DISTANCE_CM 500 // point nose at target if it is more than 5m away
#endif #endif
#define GUIDED_POSVEL_TIMEOUT_MS 3000 // guided mode's position-velocity controller times out after 3seconds with no new updates
#define GUIDED_ATTITUDE_TIMEOUT_MS 1000 // guided mode's attitude controller times out after 1 second with no new updates
static Vector3p guided_pos_target_cm; // position target (used by posvel controller only) static Vector3p guided_pos_target_cm; // position target (used by posvel controller only)
bool guided_pos_terrain_alt; // true if guided_pos_target_cm.z is an alt above terrain bool guided_pos_terrain_alt; // true if guided_pos_target_cm.z is an alt above terrain
static Vector3f guided_vel_target_cms; // velocity target (used by pos_vel_accel controller and vel_accel controller) static Vector3f guided_vel_target_cms; // velocity target (used by pos_vel_accel controller and vel_accel controller)
@ -604,7 +601,7 @@ void ModeGuided::accel_control_run()
// set velocity to zero and stop rotating if no updates received for 3 seconds // set velocity to zero and stop rotating if no updates received for 3 seconds
uint32_t tnow = millis(); uint32_t tnow = millis();
if (tnow - update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) { if (tnow - update_time_ms > get_timeout_ms()) {
guided_vel_target_cms.zero(); guided_vel_target_cms.zero();
guided_accel_target_cmss.zero(); guided_accel_target_cmss.zero();
if (auto_yaw.mode() == AUTO_YAW_RATE) { if (auto_yaw.mode() == AUTO_YAW_RATE) {
@ -667,7 +664,7 @@ void ModeGuided::velaccel_control_run()
// set velocity to zero and stop rotating if no updates received for 3 seconds // set velocity to zero and stop rotating if no updates received for 3 seconds
uint32_t tnow = millis(); uint32_t tnow = millis();
if (tnow - update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) { if (tnow - update_time_ms > get_timeout_ms()) {
guided_vel_target_cms.zero(); guided_vel_target_cms.zero();
guided_accel_target_cmss.zero(); guided_accel_target_cmss.zero();
if (auto_yaw.mode() == AUTO_YAW_RATE) { if (auto_yaw.mode() == AUTO_YAW_RATE) {
@ -742,7 +739,7 @@ void ModeGuided::posvelaccel_control_run()
// set velocity to zero and stop rotating if no updates received for 3 seconds // set velocity to zero and stop rotating if no updates received for 3 seconds
uint32_t tnow = millis(); uint32_t tnow = millis();
if (tnow - update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) { if (tnow - update_time_ms > get_timeout_ms()) {
guided_vel_target_cms.zero(); guided_vel_target_cms.zero();
guided_accel_target_cmss.zero(); guided_accel_target_cmss.zero();
if (auto_yaw.mode() == AUTO_YAW_RATE) { if (auto_yaw.mode() == AUTO_YAW_RATE) {
@ -827,7 +824,7 @@ void ModeGuided::angle_control_run()
// check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds // check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds
uint32_t tnow = millis(); uint32_t tnow = millis();
if (tnow - guided_angle_state.update_time_ms > GUIDED_ATTITUDE_TIMEOUT_MS) { if (tnow - guided_angle_state.update_time_ms > get_timeout_ms()) {
roll_in = 0.0f; roll_in = 0.0f;
pitch_in = 0.0f; pitch_in = 0.0f;
climb_rate_cms = 0.0f; climb_rate_cms = 0.0f;
@ -1037,4 +1034,10 @@ float ModeGuided::crosstrack_error() const
return 0; return 0;
} }
// return guided mode timeout in milliseconds. Only used for velocity, acceleration and angle control
uint32_t ModeGuided::get_timeout_ms() const
{
return MAX(copter.g2.guided_timeout, 0.1) * 1000;
}
#endif #endif