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