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

Copter: guided stops rotating if no updates received for 3 seconds

Browse Source
This commit is contained in:
Randy Mackay 2017-07-11 14:39:53 +09:00
parent 0021e46a79
commit a603e2485c
1 changed files with 13 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
ArduCopter/control_guided.cpp
View File

@ -481,10 +481,15 @@ void Copter::guided_vel_control_run()
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// set velocity to zero 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();
if (tnow - vel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !pos_control->get_desired_velocity().is_zero()) {
guided_set_desired_velocity_with_accel_and_fence_limits(Vector3f(0.0f,0.0f,0.0f));
if (tnow - vel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) {
if (!pos_control->get_desired_velocity().is_zero()) {
guided_set_desired_velocity_with_accel_and_fence_limits(Vector3f(0.0f,0.0f,0.0f));
}
if (auto_yaw_mode == AUTO_YAW_RATE) {
set_auto_yaw_rate(0.0f);
}
} else {
guided_set_desired_velocity_with_accel_and_fence_limits(guided_vel_target_cms);
}
@ -540,10 +545,13 @@ void Copter::guided_posvel_control_run()
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// set velocity to zero 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();
if (tnow - posvel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !guided_vel_target_cms.is_zero()) {
if (tnow - posvel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) {
guided_vel_target_cms.zero();
if (auto_yaw_mode == AUTO_YAW_RATE) {
set_auto_yaw_rate(0.0f);
}
}
// calculate dt