Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: Use new control methods for prec loiter

Browse Source
This commit is contained in:
Rishabh 2022-01-30 16:34:42 +05:30 committed by Randy Mackay
parent 819539af1a
commit 48124c1c4b
2 changed files with 29 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/mode.h
View File

@ -1079,6 +1079,7 @@ private:
#if PRECISION_LANDING == ENABLED
bool _precision_loiter_enabled;
bool _precision_loiter_active; // true if user has switched on prec loiter
#endif
};

37
ArduCopter/mode_loiter.cpp
View File

@ -34,6 +34,10 @@ bool ModeLoiter::init(bool ignore_checks)
pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
pos_control->set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
#if PRECISION_LANDING == ENABLED
_precision_loiter_active = false;
#endif
return true;
}
@ -59,15 +63,19 @@ bool ModeLoiter::do_precision_loiter()
void ModeLoiter::precision_loiter_xy()
{
loiter_nav->clear_pilot_desired_acceleration();
Vector2f target_pos, target_vel_rel;
Vector2f target_pos, target_vel;
if (!copter.precland.get_target_position_cm(target_pos)) {
target_pos = inertial_nav.get_position_xy_cm();
}
if (!copter.precland.get_target_velocity_relative_cms(target_vel_rel)) {
target_vel_rel = -inertial_nav.get_velocity_xy_cms();
}
pos_control->set_pos_target_xy_cm(target_pos.x, target_pos.y);
pos_control->override_vehicle_velocity_xy(-target_vel_rel);
// get the velocity of the target
copter.precland.get_target_velocity_cms(inertial_nav.get_velocity_xy_cms(), target_vel);
Vector2f zero;
Vector2p landing_pos = target_pos.topostype();
// target vel will remain zero if landing target is stationary
pos_control->input_pos_vel_accel_xy(landing_pos, target_vel, zero);
// run pos controller
pos_control->update_xy_controller();
}
#endif
@ -158,13 +166,24 @@ void ModeLoiter::run()
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
#if PRECISION_LANDING == ENABLED
bool precision_loiter_old_state = _precision_loiter_active;
if (do_precision_loiter()) {
precision_loiter_xy();
_precision_loiter_active = true;
} else {
_precision_loiter_active = false;
}
#endif
// run loiter controller
if (precision_loiter_old_state && !_precision_loiter_active) {
// prec loiter was active, not any more, let's init again as user takes control
loiter_nav->init_target();
}
// run loiter controller if we are not doing prec loiter
if (!_precision_loiter_active) {
loiter_nav->update();
}
#else
loiter_nav->update();
#endif
// call attitude controller
attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate);