From 59e4749fd0b9e0a1a60d1397a56eddaba7710a23 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 28 Mar 2018 11:13:37 +0900 Subject: [PATCH] Copter: integrate AC_Loiter includes param conversion --- ArduCopter/Copter.h | 2 ++ ArduCopter/Parameters.cpp | 17 ++++++++++++++++- ArduCopter/Parameters.h | 3 ++- ArduCopter/mode.cpp | 13 +++++++------ ArduCopter/mode.h | 1 + ArduCopter/mode_auto.cpp | 16 ++++++++-------- ArduCopter/mode_brake.cpp | 5 +---- ArduCopter/mode_land.cpp | 6 +++--- ArduCopter/mode_loiter.cpp | 34 +++++++++++++++++----------------- ArduCopter/mode_poshold.cpp | 22 +++++++++++----------- ArduCopter/mode_rtl.cpp | 16 ++++++++-------- ArduCopter/mode_throw.cpp | 6 +++--- ArduCopter/system.cpp | 7 +++++++ 13 files changed, 86 insertions(+), 62 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 819e6df209..5a2c89b4b9 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -68,6 +68,7 @@ #include // needed for AHRS build #include // ArduPilot Mega inertial navigation library #include // ArduCopter waypoint navigation library +#include #include // circle navigation library #include // ArduPilot Mega Declination Helper Library #include // main loop scheduler @@ -501,6 +502,7 @@ private: AC_AttitudeControl_t *attitude_control; AC_PosControl *pos_control; AC_WPNav *wp_nav; + AC_Loiter *loiter_nav; #if MODE_CIRCLE_ENABLED == ENABLED AC_Circle *circle_nav; #endif diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index ab28a5ef50..5235700cd6 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -578,6 +578,10 @@ const AP_Param::Info Copter::var_info[] = { // @Path: ../libraries/AC_WPNav/AC_WPNav.cpp GOBJECTPTR(wp_nav, "WPNAV_", AC_WPNav), + // @Group: LOIT_ + // @Path: ../libraries/AC_WPNav/AC_Loiter.cpp + GOBJECTPTR(loiter_nav, "LOIT_", AC_Loiter), + #if MODE_CIRCLE_ENABLED == ENABLED // @Group: CIRCLE_ // @Path: ../libraries/AC_WPNav/AC_Circle.cpp @@ -1043,7 +1047,7 @@ const AP_Param::ConversionInfo conversion_table[] = { // battery { Parameters::k_param_fs_batt_voltage, 0, AP_PARAM_INT8, "BATT_FS_LOW_VOLT" }, { Parameters::k_param_fs_batt_mah, 0, AP_PARAM_INT8, "BATT_FS_LOW_MAH" }, - { Parameters::k_param_failsafe_battery_enabled, 0, AP_PARAM_INT8, "BATT_FS_LOW_ACT" }, + { Parameters::k_param_failsafe_battery_enabled,0, AP_PARAM_INT8, "BATT_FS_LOW_ACT" }, }; void Copter::load_parameters(void) @@ -1134,6 +1138,12 @@ void Copter::convert_pid_parameters(void) { Parameters::k_param_throttle_min, 0, AP_PARAM_FLOAT, "MOT_SPIN_MIN" }, { Parameters::k_param_throttle_mid, 0, AP_PARAM_FLOAT, "MOT_THST_HOVER" } }; + const AP_Param::ConversionInfo loiter_conversion_info[] = { + { Parameters::k_param_wp_nav, 4, AP_PARAM_FLOAT, "LOIT_SPEED" }, + { Parameters::k_param_wp_nav, 7, AP_PARAM_FLOAT, "LOIT_BRK_JERK" }, + { Parameters::k_param_wp_nav, 8, AP_PARAM_FLOAT, "LOIT_ACC_MAX" }, + { Parameters::k_param_wp_nav, 9, AP_PARAM_FLOAT, "LOIT_BRK_ACCEL" } + }; // gains increase by 27% due to attitude controller's switch to use radians instead of centi-degrees // and motor libraries switch to accept inputs in -1 to +1 range instead of -4500 ~ +4500 @@ -1172,6 +1182,11 @@ void Copter::convert_pid_parameters(void) if (AP_Param::find_old_parameter(&rc_feel_rp_conversion_info, &rc_feel_rp_old)) { AP_Param::set_default_by_name(rc_feel_rp_conversion_info.new_name, (1.0f / (2.0f + rc_feel_rp_old.get() * 0.1f))); } + // convert loiter parameters + table_size = ARRAY_SIZE(loiter_conversion_info); + for (uint8_t i=0; iloiter_soften_for_landing(); + loiter_nav->soften_for_landing(); } // process pilot inputs @@ -472,7 +473,7 @@ void Copter::Mode::land_run_horizontal_control() update_simple_mode(); // convert pilot input to lean angles - get_pilot_desired_lean_angles(target_roll, target_pitch, wp_nav->get_loiter_angle_max_cd(), attitude_control->get_althold_lean_angle_max()); + get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max()); // record if pilot has overriden roll or pitch if (!is_zero(target_roll) || !is_zero(target_pitch)) { @@ -503,13 +504,13 @@ void Copter::Mode::land_run_horizontal_control() #endif // process roll, pitch inputs - wp_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt); + loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt); // run loiter controller - wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); + loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler); - int32_t nav_roll = wp_nav->get_roll(); - int32_t nav_pitch = wp_nav->get_pitch(); + int32_t nav_roll = loiter_nav->get_roll(); + int32_t nav_pitch = loiter_nav->get_pitch(); if (g2.wp_navalt_min > 0) { // user has requested an altitude below which navigation diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index da6b281212..da628a7ddb 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -54,6 +54,7 @@ protected: Parameters &g; ParametersG2 &g2; AC_WPNav *&wp_nav; + AC_Loiter *&loiter_nav; AC_PosControl *&pos_control; AP_InertialNav &inertial_nav; AP_AHRS &ahrs; diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index bcb1618108..667bffa602 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -218,7 +218,7 @@ void Copter::ModeAuto::land_start() { // set target to stopping point Vector3f stopping_point; - wp_nav->get_loiter_stopping_point_xy(stopping_point); + loiter_nav->get_stopping_point_xy(stopping_point); // call location specific land start function land_start(stopping_point); @@ -230,7 +230,7 @@ void Copter::ModeAuto::land_start(const Vector3f& destination) _mode = Auto_Land; // initialise loiter target destination - wp_nav->init_loiter_target(destination); + loiter_nav->init_target(destination); // initialise position and desired velocity if (!pos_control->is_active_z()) { @@ -360,7 +360,7 @@ void Copter::ModeAuto::payload_place_start() { // set target to stopping point Vector3f stopping_point; - wp_nav->get_loiter_stopping_point_xy(stopping_point); + loiter_nav->get_stopping_point_xy(stopping_point); // call location specific place start function payload_place_start(stopping_point); @@ -852,7 +852,7 @@ void Copter::ModeAuto::land_run() if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) { zero_throttle_and_relax_ac(); // set target to current position - wp_nav->init_loiter_target(); + loiter_nav->init_target(); return; } @@ -928,7 +928,7 @@ void Copter::ModeAuto::payload_place_start(const Vector3f& destination) nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start; // initialise loiter target destination - wp_nav->init_loiter_target(destination); + loiter_nav->init_target(destination); // initialise position and desired velocity if (!pos_control->is_active_z()) { @@ -947,7 +947,7 @@ void Copter::ModeAuto::payload_place_run() if (!payload_place_run_should_run()) { zero_throttle_and_relax_ac(); // set target to current position - wp_nav->init_loiter_target(); + loiter_nav->init_target(); return; } @@ -1000,7 +1000,7 @@ void Copter::ModeAuto::payload_place_run_loiter() land_run_horizontal_control(); // run loiter controller - wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); + loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler); // call attitude controller const float target_yaw_rate = 0; @@ -2060,4 +2060,4 @@ float Copter::get_auto_yaw_rate_cds(void) // return zero turn rate (this should never happen) return 0.0f; -} \ No newline at end of file +} diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp index 3bbb0a2bba..ec6ddf6ef7 100644 --- a/ArduCopter/mode_brake.cpp +++ b/ArduCopter/mode_brake.cpp @@ -9,9 +9,6 @@ bool Copter::ModeBrake::init(bool ignore_checks) { if (copter.position_ok() || ignore_checks) { - // set desired acceleration to zero - wp_nav->clear_pilot_desired_acceleration(); - // set target to current position wp_nav->init_brake_target(BRAKE_MODE_DECEL_RATE); @@ -47,7 +44,7 @@ void Copter::ModeBrake::run() // relax stop target if we might be landed if (ap.land_complete_maybe) { - wp_nav->loiter_soften_for_landing(); + loiter_nav->soften_for_landing(); } // if landed immediately disarm diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index 7ccdf7dcb0..74485bf904 100644 --- a/ArduCopter/mode_land.cpp +++ b/ArduCopter/mode_land.cpp @@ -14,8 +14,8 @@ bool Copter::ModeLand::init(bool ignore_checks) if (land_with_gps) { // set target to stopping point Vector3f stopping_point; - wp_nav->get_loiter_stopping_point_xy(stopping_point); - wp_nav->init_loiter_target(stopping_point); + loiter_nav->get_stopping_point_xy(stopping_point); + loiter_nav->init_target(stopping_point); } // initialize vertical speeds and leash lengths @@ -57,7 +57,7 @@ void Copter::ModeLand::gps_run() // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) { zero_throttle_and_relax_ac(); - wp_nav->init_loiter_target(); + loiter_nav->init_target(); // disarm when the landing detector says we've landed if (ap.land_complete) { diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index d20951c5d5..5e0cdc00ed 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -10,7 +10,7 @@ bool Copter::ModeLoiter::init(bool ignore_checks) if (copter.position_ok() || ignore_checks) { // set target to current position - wp_nav->init_loiter_target(); + loiter_nav->init_target(); // initialise position and desired velocity if (!pos_control->is_active_z()) { @@ -34,7 +34,7 @@ bool Copter::ModeLoiter::do_precision_loiter() 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) { + if (loiter_nav->get_pilot_desired_acceleration().length() > 50.0f) { return false; } if (!copter.precland.target_acquired()) { @@ -45,7 +45,7 @@ bool Copter::ModeLoiter::do_precision_loiter() void Copter::ModeLoiter::precision_loiter_xy() { - wp_nav->clear_pilot_desired_acceleration(); + loiter_nav->clear_pilot_desired_acceleration(); Vector2f target_pos, target_vel_rel; if (!copter.precland.get_target_position_cm(target_pos)) { target_pos.x = inertial_nav.get_position().x; @@ -81,10 +81,10 @@ void Copter::ModeLoiter::run() update_simple_mode(); // convert pilot input to lean angles - get_pilot_desired_lean_angles(target_roll, target_pitch, wp_nav->get_loiter_angle_max_cd(), attitude_control->get_althold_lean_angle_max()); + get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max()); // process pilot's roll and pitch input - wp_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt); + loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt); // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); @@ -94,12 +94,12 @@ void Copter::ModeLoiter::run() target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up); } else { // clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason - wp_nav->clear_pilot_desired_acceleration(); + loiter_nav->clear_pilot_desired_acceleration(); } // relax loiter target if we might be landed if (ap.land_complete_maybe) { - wp_nav->loiter_soften_for_landing(); + loiter_nav->soften_for_landing(); } // Loiter State Machine Determination @@ -123,13 +123,13 @@ void Copter::ModeLoiter::run() // force descent rate and call position controller pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false); #else - wp_nav->init_loiter_target(); + loiter_nav->init_target(); attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero #endif - wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate); + loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate); pos_control->update_z_controller(); break; @@ -153,10 +153,10 @@ void Copter::ModeLoiter::run() target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); // run loiter controller - wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); + loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler); // call attitude controller - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate); // update altitude target and call position controller pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); @@ -171,7 +171,7 @@ void Copter::ModeLoiter::run() } else { motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); } - wp_nav->init_loiter_target(); + loiter_nav->init_target(); attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0); @@ -191,10 +191,10 @@ void Copter::ModeLoiter::run() #endif // run loiter controller - wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); + loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler); // call attitude controller - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate); // adjust climb rate using rangefinder if (copter.rangefinder_alt_ok()) { @@ -214,10 +214,10 @@ void Copter::ModeLoiter::run() uint32_t Copter::ModeLoiter::wp_distance() const { - return wp_nav->get_loiter_distance_to_target(); + return loiter_nav->get_distance_to_target(); } int32_t Copter::ModeLoiter::wp_bearing() const { - return wp_nav->get_loiter_bearing_to_target(); + return loiter_nav->get_bearing_to_target(); } diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index 6669a1c370..0a7bb38c67 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -100,7 +100,7 @@ bool Copter::ModePosHold::init(bool ignore_checks) poshold.pitch_mode = POSHOLD_LOITER; // set target to current position // only init here as we can switch to PosHold in flight with a velocity <> 0 that will be used as _last_vel in PosControl and never updated again as we inhibit Reset_I - wp_nav->init_loiter_target(); + loiter_nav->init_target(); }else{ // if not landed start in pilot override to avoid hard twitch poshold.roll_mode = POSHOLD_PILOT_OVERRIDE; @@ -136,7 +136,7 @@ void Copter::ModePosHold::run() // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) { - wp_nav->init_loiter_target(); + loiter_nav->init_target(); zero_throttle_and_relax_ac(); pos_control->relax_alt_hold_controllers(0.0f); return; @@ -177,7 +177,7 @@ void Copter::ModePosHold::run() // relax loiter target if we might be landed if (ap.land_complete_maybe) { - wp_nav->loiter_soften_for_landing(); + loiter_nav->soften_for_landing(); } // if landed initialise loiter targets, set throttle to zero and exit @@ -188,7 +188,7 @@ void Copter::ModePosHold::run() } else { motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); } - wp_nav->init_loiter_target(); + loiter_nav->init_target(); attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0); @@ -409,7 +409,7 @@ void Copter::ModePosHold::run() poshold.pitch_mode = POSHOLD_BRAKE_TO_LOITER; poshold.brake_to_loiter_timer = POSHOLD_BRAKE_TO_LOITER_TIMER; // init loiter controller - wp_nav->init_loiter_target(inertial_nav.get_position()); + loiter_nav->init_target(inertial_nav.get_position()); // set delay to start of wind compensation estimate updates poshold.wind_comp_start_timer = POSHOLD_WIND_COMP_START_TIMER; } @@ -440,11 +440,11 @@ void Copter::ModePosHold::run() poshold_update_brake_angle_from_velocity(poshold.brake_pitch, -vel_fw); // run loiter controller - wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); + loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler); // calculate final roll and pitch output by mixing loiter and brake controls - poshold.roll = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_roll + poshold.wind_comp_roll, wp_nav->get_roll()); - poshold.pitch = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_pitch + poshold.wind_comp_pitch, wp_nav->get_pitch()); + poshold.roll = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_roll + poshold.wind_comp_roll, loiter_nav->get_roll()); + poshold.pitch = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_pitch + poshold.wind_comp_pitch, loiter_nav->get_pitch()); // check for pilot input if (!is_zero(target_roll) || !is_zero(target_pitch)) { @@ -471,11 +471,11 @@ void Copter::ModePosHold::run() case POSHOLD_LOITER: // run loiter controller - wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); + loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler); // set roll angle based on loiter controller outputs - poshold.roll = wp_nav->get_roll(); - poshold.pitch = wp_nav->get_pitch(); + poshold.roll = loiter_nav->get_roll(); + poshold.pitch = loiter_nav->get_pitch(); // update wind compensation estimate poshold_update_wind_comp_estimate(); diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index cab8f9b160..cd85d694f5 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -244,7 +244,7 @@ void Copter::ModeRTL::descent_start() _state_complete = false; // Set wp navigation target to above home - wp_nav->init_loiter_target(wp_nav->get_wp_destination()); + loiter_nav->init_target(wp_nav->get_wp_destination()); // initialise altitude target to stopping point pos_control->set_target_to_stopping_point_z(); @@ -265,7 +265,7 @@ void Copter::ModeRTL::descent_run() if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) { zero_throttle_and_relax_ac(); // set target to current position - wp_nav->init_loiter_target(); + loiter_nav->init_target(); return; } @@ -284,7 +284,7 @@ void Copter::ModeRTL::descent_run() update_simple_mode(); // convert pilot input to lean angles - get_pilot_desired_lean_angles(target_roll, target_pitch, wp_nav->get_loiter_angle_max_cd(), attitude_control->get_althold_lean_angle_max()); + get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max()); // record if pilot has overriden roll or pitch if (!is_zero(target_roll) || !is_zero(target_pitch)) { @@ -300,17 +300,17 @@ void Copter::ModeRTL::descent_run() motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // process roll, pitch inputs - wp_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt); + loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt); // run loiter controller - wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); + loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler); // call z-axis position controller pos_control->set_alt_target_with_slew(rtl_path.descent_target.alt, G_Dt); pos_control->update_z_controller(); // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate); // check if we've reached within 20cm of final altitude _state_complete = labs(rtl_path.descent_target.alt - copter.current_loc.alt) < 20; @@ -323,7 +323,7 @@ void Copter::ModeRTL::land_start() _state_complete = false; // Set wp navigation target to above home - wp_nav->init_loiter_target(wp_nav->get_wp_destination()); + loiter_nav->init_target(wp_nav->get_wp_destination()); // initialise position and desired velocity if (!pos_control->is_active_z()) { @@ -356,7 +356,7 @@ void Copter::ModeRTL::land_run(bool disarm_on_land) if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) { zero_throttle_and_relax_ac(); // set target to current position - wp_nav->init_loiter_target(); + loiter_nav->init_target(); // disarm when the landing detector says we've landed if (ap.land_complete && disarm_on_land) { diff --git a/ArduCopter/mode_throw.cpp b/ArduCopter/mode_throw.cpp index 56e4688964..c5f7850ee3 100644 --- a/ArduCopter/mode_throw.cpp +++ b/ArduCopter/mode_throw.cpp @@ -79,7 +79,7 @@ void Copter::ModeThrow::run() stage = Throw_PosHold; // initialise the loiter target to the curent position and velocity - wp_nav->init_loiter_target(); + loiter_nav->init_target(); // Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum copter.set_auto_armed(true); @@ -168,10 +168,10 @@ void Copter::ModeThrow::run() motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // run loiter controller - wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); + loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler); // call attitude controller - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), 0.0f); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), 0.0f); // call height controller pos_control->set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false); diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 7648710be1..8fbb2b613c 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -170,6 +170,7 @@ void Copter::init_ardupilot() #endif #if AC_AVOID_ENABLED == ENABLED wp_nav->set_avoidance(&avoid); + loiter_nav->set_avoidance(&avoid); #endif attitude_control->parameter_sanity_check(); @@ -602,6 +603,12 @@ void Copter::allocate_motors(void) } AP_Param::load_object_from_eeprom(wp_nav, wp_nav->var_info); + loiter_nav = new AC_Loiter(inertial_nav, *ahrs_view, *pos_control, *attitude_control); + if (loiter_nav == nullptr) { + AP_HAL::panic("Unable to allocate LoiterNav"); + } + AP_Param::load_object_from_eeprom(loiter_nav, loiter_nav->var_info); + #if MODE_CIRCLE_ENABLED == ENABLED circle_nav = new AC_Circle(inertial_nav, *ahrs_view, *pos_control); if (circle_nav == nullptr) {