Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 08:13:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: precision loiter

Browse Source
This commit is contained in:
Peter Barker 2016-11-14 13:46:47 +11:00 committed by Randy Mackay
parent fd5f79b238
commit 9e4628af58
2 changed files with 41 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/Copter.h
View File

@ -883,6 +883,8 @@ private:
bool landing_with_GPS();
bool loiter_init(bool ignore_checks);
void loiter_run();
bool do_precision_loiter() const;
void precision_loiter_xy();
bool poshold_init(bool ignore_checks);
void poshold_run();
void poshold_update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw);

39
ArduCopter/control_loiter.cpp
View File

@ -35,6 +35,41 @@ bool Copter::loiter_init(bool ignore_checks)
}
}
bool Copter::do_precision_loiter() const
{
#if PRECISION_LANDING != ENABLED
return false;
#else
if (ap.land_complete_maybe) {
return false; // don't move on the ground
}
// if the pilot *really* wants to move the vehicle, let them....
if (wp_nav.get_pilot_desired_acceleration().length() > 50.0f) {
return false;
}
if (!precland.target_acquired()) {
return false; // we don't have a good vector
}
return true;
#endif
}
void Copter::precision_loiter_xy()
{
wp_nav.clear_pilot_desired_acceleration();
Vector2f target_pos, target_vel_rel;
if (!precland.get_target_position_cm(target_pos)) {
target_pos.x = inertial_nav.get_position().x;
target_pos.y = inertial_nav.get_position().y;
}
if (!precland.get_target_velocity_relative_cms(target_vel_rel)) {
target_vel_rel.x = -inertial_nav.get_velocity().x;
target_vel_rel.y = -inertial_nav.get_velocity().y;
}
pos_control.set_xy_target(target_pos.x, target_pos.y);
pos_control.override_vehicle_velocity_xy(-target_vel_rel);
}
// loiter_run - runs the loiter controller
// should be called at 100hz or more
void Copter::loiter_run()
@ -158,6 +193,10 @@ void Copter::loiter_run()
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
if (do_precision_loiter()) {
precision_loiter_xy();
}
// run loiter controller
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);