mirror of https://github.com/ArduPilot/ardupilot
parent
e150ff369b
commit
556eb88fe5
|
@ -909,10 +909,12 @@ private:
|
|||
bool landing_with_GPS();
|
||||
bool loiter_init(bool ignore_checks);
|
||||
void loiter_run();
|
||||
#if PRECISION_LANDING == ENABLED
|
||||
bool do_precision_loiter() const;
|
||||
void precision_loiter_xy();
|
||||
void set_precision_loiter_enabled(bool value) { _precision_loiter_enabled = value; }
|
||||
bool _precision_loiter_enabled;
|
||||
#endif
|
||||
bool poshold_init(bool ignore_checks);
|
||||
void poshold_run();
|
||||
void poshold_update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw);
|
||||
|
|
|
@ -35,11 +35,9 @@ bool Copter::loiter_init(bool ignore_checks)
|
|||
}
|
||||
}
|
||||
|
||||
#if PRECISION_LANDING == ENABLED
|
||||
bool Copter::do_precision_loiter() const
|
||||
{
|
||||
#if PRECISION_LANDING != ENABLED
|
||||
return false;
|
||||
#else
|
||||
if (!_precision_loiter_enabled) {
|
||||
return false;
|
||||
}
|
||||
|
@ -54,7 +52,6 @@ bool Copter::do_precision_loiter() const
|
|||
return false; // we don't have a good vector
|
||||
}
|
||||
return true;
|
||||
#endif
|
||||
}
|
||||
|
||||
void Copter::precision_loiter_xy()
|
||||
|
@ -72,6 +69,7 @@ void Copter::precision_loiter_xy()
|
|||
pos_control.set_xy_target(target_pos.x, target_pos.y);
|
||||
pos_control.override_vehicle_velocity_xy(-target_vel_rel);
|
||||
}
|
||||
#endif
|
||||
|
||||
// loiter_run - runs the loiter controller
|
||||
// should be called at 100hz or more
|
||||
|
@ -196,9 +194,11 @@ void Copter::loiter_run()
|
|||
// set motors to full range
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
#if PRECISION_LANDING == ENABLED
|
||||
if (do_precision_loiter()) {
|
||||
precision_loiter_xy();
|
||||
}
|
||||
#endif
|
||||
|
||||
// run loiter controller
|
||||
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
|
|
|
@ -550,6 +550,7 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
|
|||
break;
|
||||
|
||||
case AUXSW_PRECISION_LOITER:
|
||||
#if PRECISION_LANDING == ENABLED
|
||||
switch (ch_flag) {
|
||||
case AUX_SWITCH_HIGH:
|
||||
set_precision_loiter_enabled(true);
|
||||
|
@ -558,6 +559,7 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
|
|||
set_precision_loiter_enabled(false);
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue