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

Copter: FlightMode - convert LOITER flight mode

Browse Source
This commit is contained in:
Peter Barker 2016-03-22 11:42:21 +11:00 committed by Randy Mackay
parent 871ba5630f
commit f2495b2d08
5 changed files with 58 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduCopter/Copter.h
View File

@ -876,14 +876,6 @@ private:
void land_do_not_use_GPS();
void set_mode_land_with_pause(mode_reason_t reason);
bool landing_with_GPS();
bool loiter_init(bool ignore_checks);
void loiter_run();
#if PRECISION_LANDING == ENABLED
bool do_precision_loiter();
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);
@ -1136,6 +1128,8 @@ private:
Copter::FlightMode_CIRCLE flightmode_circle{*this, circle_nav};
Copter::FlightMode_LOITER flightmode_loiter{*this};
#if FRAME_CONFIG == HELI_FRAME
Copter::FlightMode_STABILIZE_Heli flightmode_stabilize{*this};
#else

40
ArduCopter/FlightMode.h
View File

@ -373,3 +373,43 @@ private:
AC_Circle *&circle_nav;
};
class FlightMode_LOITER : public FlightMode {
public:
FlightMode_LOITER(Copter &copter) :
Copter::FlightMode(copter)
{ }
bool init(bool ignore_checks) override;
void run() override; // should be called at 100hz or more
bool requires_GPS() const override { return true; }
bool has_manual_throttle() const override { return false; }
bool allows_arming(bool from_gcs) const override { return true; };
bool is_autopilot() const override { return false; }
#if PRECISION_LANDING == ENABLED
void set_precision_loiter_enabled(bool value) { _precision_loiter_enabled = value; }
#endif
protected:
const char *name() const override { return "LOITER"; }
const char *name4() const override { return "LOIT"; }
#if PRECISION_LANDING == ENABLED
bool do_precision_loiter();
void precision_loiter_xy();
#endif
private:
#if PRECISION_LANDING == ENABLED
bool _precision_loiter_enabled;
#endif
};

20
ArduCopter/control_loiter.cpp
View File

@ -5,9 +5,9 @@
*/
// loiter_init - initialise loiter controller
bool Copter::loiter_init(bool ignore_checks)
bool Copter::FlightMode_LOITER::init(bool ignore_checks)
{
if (position_ok() || ignore_checks) {
if (_copter.position_ok() || ignore_checks) {
// set target to current position
wp_nav->init_loiter_target();
@ -29,7 +29,7 @@ bool Copter::loiter_init(bool ignore_checks)
}
#if PRECISION_LANDING == ENABLED
bool Copter::do_precision_loiter()
bool Copter::FlightMode_LOITER::do_precision_loiter()
{
if (!_precision_loiter_enabled) {
return false;
@ -41,21 +41,21 @@ bool Copter::do_precision_loiter()
if (wp_nav->get_pilot_desired_acceleration().length() > 50.0f) {
return false;
}
if (!precland.target_acquired()) {
if (!_copter.precland.target_acquired()) {
return false; // we don't have a good vector
}
return true;
}
void Copter::precision_loiter_xy()
void Copter::FlightMode_LOITER::precision_loiter_xy()
{
wp_nav->clear_pilot_desired_acceleration();
Vector2f target_pos, target_vel_rel;
if (!precland.get_target_position_cm(target_pos)) {
if (!_copter.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)) {
if (!_copter.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;
}
@ -66,7 +66,7 @@ void Copter::precision_loiter_xy()
// loiter_run - runs the loiter controller
// should be called at 100hz or more
void Copter::loiter_run()
void Copter::FlightMode_LOITER::run()
{
LoiterModeState loiter_state;
float target_yaw_rate = 0.0f;
@ -78,7 +78,7 @@ void Copter::loiter_run()
pos_control->set_accel_z(g.pilot_accel_z);
// process pilot inputs unless we are in radio failsafe
if (!failsafe.radio) {
if (!_copter.failsafe.radio) {
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
@ -203,7 +203,7 @@ void Copter::loiter_run()
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
// adjust climb rate using rangefinder
if (rangefinder_alt_ok()) {
if (_copter.rangefinder_alt_ok()) {
// if rangefinder is ok, use surface tracking
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
}

12
ArduCopter/flight_mode.cpp
View File

@ -75,7 +75,10 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
break;
case LOITER:
success = loiter_init(ignore_checks);
success = flightmode_loiter.init(ignore_checks);
if (success) {
flightmode = &flightmode_loiter;
}
break;
case GUIDED:
@ -199,9 +202,6 @@ void Copter::update_flight_mode()
}
switch (control_mode) {
case LOITER:
loiter_run();
break;
case GUIDED:
guided_run();
@ -326,7 +326,6 @@ bool Copter::mode_requires_GPS()
}
switch (control_mode) {
case GUIDED:
case LOITER:
case RTL:
case SMART_RTL:
case DRIFT:
@ -397,9 +396,6 @@ void Copter::notify_flight_mode()
case GUIDED:
notify.set_flight_mode_str("GUID");
break;
case LOITER:
notify.set_flight_mode_str("LOIT");
break;
case RTL:
notify.set_flight_mode_str("RTL ");
break;

4
ArduCopter/switches.cpp
View File

@ -562,10 +562,10 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
#if PRECISION_LANDING == ENABLED
switch (ch_flag) {
case AUX_SWITCH_HIGH:
set_precision_loiter_enabled(true);
flightmode_loiter.set_precision_loiter_enabled(true);
break;
case AUX_SWITCH_LOW:
set_precision_loiter_enabled(false);
flightmode_loiter.set_precision_loiter_enabled(false);
break;
}
#endif