Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-19 14:23:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: integrate AC_Loiter

Browse Source 
includes param conversion
This commit is contained in:
Randy Mackay 2018-03-28 11:13:37 +09:00
parent 5f2f446199
commit 59e4749fd0
13 changed files with 86 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/Copter.h
View File

@ -68,6 +68,7 @@
#include <AP_Vehicle/AP_Vehicle.h> // needed for AHRS build
#include <AP_InertialNav/AP_InertialNav.h> // ArduPilot Mega inertial navigation library
#include <AC_WPNav/AC_WPNav.h> // ArduCopter waypoint navigation library
#include <AC_WPNav/AC_Loiter.h>
#include <AC_WPNav/AC_Circle.h> // circle navigation library
#include <AP_Declination/AP_Declination.h> // ArduPilot Mega Declination Helper Library
#include <AP_Scheduler/AP_Scheduler.h> // 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

17
ArduCopter/Parameters.cpp
View File

@ -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; i<table_size; i++) {
AP_Param::convert_old_parameter(&loiter_conversion_info[i], 1.0f);
}
const uint8_t old_rc_keys[14] = { Parameters::k_param_rc_1_old, Parameters::k_param_rc_2_old,
Parameters::k_param_rc_3_old, Parameters::k_param_rc_4_old,

3
ArduCopter/Parameters.h
View File

@ -196,7 +196,8 @@ public:
k_param_wp_nav,
k_param_attitude_control,
k_param_pos_control,
k_param_circle_nav, // 104
k_param_circle_nav,
k_param_loiter_nav, // 105
// 110: Telemetry control
//

13
ArduCopter/mode.cpp
View File

@ -12,6 +12,7 @@ Copter::Mode::Mode(void) :
g(copter.g),
g2(copter.g2),
wp_nav(copter.wp_nav),
loiter_nav(copter.loiter_nav),
pos_control(copter.pos_control),
inertial_nav(copter.inertial_nav),
ahrs(copter.ahrs),
@ -454,7 +455,7 @@ void Copter::Mode::land_run_horizontal_control()
// relax loiter target if we might be landed
if (ap.land_complete_maybe) {
wp_nav->loiter_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

1
ArduCopter/mode.h
View File

@ -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;

16
ArduCopter/mode_auto.cpp
View File

@ -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;
}
}

5
ArduCopter/mode_brake.cpp
View File

@ -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

6
ArduCopter/mode_land.cpp
View File

@ -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) {

34
ArduCopter/mode_loiter.cpp
View File

@ -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();
}

22
ArduCopter/mode_poshold.cpp
View File

@ -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();

16
ArduCopter/mode_rtl.cpp
View File

@ -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) {

6
ArduCopter/mode_throw.cpp
View File

@ -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);

7
ArduCopter/system.cpp
View File

@ -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) {