aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: add velocity control timeout in guided mode

Browse Source
This commit is contained in:
TShapinsky 2015-08-11 16:36:42 -07:00 committed by Randy Mackay
parent f4d319ba83
commit 5dfd1ff8d6
1 changed files with 9 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
ArduCopter/control_guided.cpp
View File

@ -15,6 +15,7 @@
static Vector3f posvel_pos_target_cm;
static Vector3f posvel_vel_target_cms;
static uint32_t posvel_update_time_ms;
static uint32_t vel_update_time_ms;
struct Guided_Limit {
uint32_t timeout_ms; // timeout (in seconds) from the time that guided is invoked
@ -138,6 +139,8 @@ void Copter::guided_set_velocity(const Vector3f& velocity)
guided_vel_control_start();
}
vel_update_time_ms = millis();
// set position controller velocity target
pos_control.set_desired_velocity(velocity);
}
@ -276,6 +279,12 @@ void Copter::guided_vel_control_run()
}
}
// set velocity to zero 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()) {
pos_control.set_desired_velocity(Vector3f(0,0,0));
}
// calculate dt
float dt = pos_control.time_since_last_xy_update();