Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: remove loiter_nav from auto

This commit is contained in:
Leonard Hall 2022-02-22 23:53:00 +10:30 committed by Randy Mackay
parent 56e47cb8cc
commit 93cff95448
5 changed files with 60 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduCopter/mode.cpp
View File

@ -696,14 +696,6 @@ void Mode::land_run_horizontal_control()
#endif
if (!copter.ap.prec_land_active) {
if (copter.ap.prec_land_active) {
// precland isn't active anymore but was active a while back
// lets run an init again
// set target to stopping point
Vector2f stopping_point;
loiter_nav->get_stopping_point_xy(stopping_point);
loiter_nav->init_target(stopping_point);
}
Vector2f accel;
pos_control->input_vel_accel_xy(vel_correction, accel);
}

2
ArduCopter/mode.h
View File

@ -424,7 +424,6 @@ public:
void takeoff_start(const Location& dest_loc);
void wp_start(const Location& dest_loc);
void land_start();
void land_start(const Vector2f& destination);
void circle_movetoedge_start(const Location &circle_center, float radius_m);
void circle_start();
void nav_guided_start();
@ -495,7 +494,6 @@ private:
Location loc_from_cmd(const AP_Mission::Mission_Command& cmd, const Location& default_loc) const;
void payload_place_start(const Vector2f& destination);
void payload_place_run();
bool payload_place_run_should_run();
void payload_place_run_loiter();

89
ArduCopter/mode_auto.cpp
View File

@ -343,22 +343,21 @@ void ModeAuto::wp_start(const Location& dest_loc)
// auto_land_start - initialises controller to implement a landing
void ModeAuto::land_start()
{
// set target to stopping point
Vector2f stopping_point;
loiter_nav->get_stopping_point_xy(stopping_point);
// call location specific land start function
land_start(stopping_point);
}
// auto_land_start - initialises controller to implement a landing
void ModeAuto::land_start(const Vector2f& destination)
{
_mode = SubMode::LAND;
// initialise loiter target destination
loiter_nav->init_target(destination);
// set horizontal speed and acceleration limits
pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
// initialise the vertical position controller
if (!pos_control->is_active_xy()) {
pos_control->init_xy_controller();
}
// set vertical speed and acceleration limits
pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z());
pos_control->set_correction_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z());
// initialise the vertical position controller
if (!pos_control->is_active_z()) {
@ -485,12 +484,29 @@ bool ModeAuto::is_taking_off() const
// auto_payload_place_start - initialises controller to implement a placing
void ModeAuto::payload_place_start()
{
// set target to stopping point
Vector2f stopping_point;
loiter_nav->get_stopping_point_xy(stopping_point);
_mode = SubMode::NAV_PAYLOAD_PLACE;
nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start;
// call location specific place start function
payload_place_start(stopping_point);
// set horizontal speed and acceleration limits
pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
// initialise the vertical position controller
if (!pos_control->is_active_xy()) {
pos_control->init_xy_controller();
}
// set vertical speed and acceleration limits
pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z());
pos_control->set_correction_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z());
// initialise the vertical position controller
if (!pos_control->is_active_z()) {
pos_control->init_z_controller();
}
// initialise yaw
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
// returns true if pilot's yaw input should be used to adjust vehicle's heading
@ -908,8 +924,6 @@ void ModeAuto::land_run()
// if not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) {
make_safe_ground_handling();
loiter_nav->clear_pilot_desired_acceleration();
loiter_nav->init_target();
return;
}
@ -1015,8 +1029,14 @@ void ModeAuto::loiter_to_alt_run()
}
if (!loiter_to_alt.loiter_start_done) {
loiter_nav->clear_pilot_desired_acceleration();
loiter_nav->init_target();
// set horizontal speed and acceleration limits
pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
if (!pos_control->is_active_xy()) {
pos_control->init_xy_controller();
}
_mode = SubMode::LOITER_TO_ALT;
loiter_to_alt.loiter_start_done = true;
}
@ -1054,31 +1074,12 @@ void ModeAuto::loiter_to_alt_run()
pos_control->update_z_controller();
}
// auto_payload_place_start - initialises controller to implement placement of a load
void ModeAuto::payload_place_start(const Vector2f& destination)
{
_mode = SubMode::NAV_PAYLOAD_PLACE;
nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start;
// initialise loiter target destination
loiter_nav->init_target(destination);
// initialise the vertical position controller
pos_control->init_z_controller();
// initialise yaw
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
// auto_payload_place_run - places an object in auto mode
// called by auto_run at 100hz or more
void ModeAuto::payload_place_run()
{
if (!payload_place_run_should_run()) {
zero_throttle_and_relax_ac();
// set target to current position
loiter_nav->clear_pilot_desired_acceleration();
loiter_nav->init_target();
return;
}
@ -1409,6 +1410,7 @@ void ModeAuto::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd)
// set vertical speed and acceleration limits
pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z());
pos_control->set_correction_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z());
}
// do_spline_wp - initiate move to next waypoint
@ -1685,11 +1687,8 @@ bool ModeAuto::verify_land()
case State::FlyToLocation:
// check if we've reached the location
if (copter.wp_nav->reached_wp_destination()) {
// get destination so we can use it for loiter target
const Vector2f& dest = copter.wp_nav->get_wp_destination().xy();
// initialise landing controller
land_start(dest);
land_start();
// advance to next state
state = State::Descending;

16
ArduCopter/mode_land.cpp
View File

@ -5,17 +5,19 @@ bool ModeLand::init(bool ignore_checks)
{
// check if we have GPS and decide which LAND we're going to do
control_position = copter.position_ok();
if (control_position) {
// set target to stopping point
Vector2f stopping_point;
loiter_nav->get_stopping_point_xy(stopping_point);
loiter_nav->init_target(stopping_point);
// set horizontal speed and acceleration limits
pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
// initialise the horizontal position controller
if (control_position && !pos_control->is_active_xy()) {
pos_control->init_xy_controller();
}
// set vertical speed and acceleration limits
pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z());
pos_control->set_correction_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z());
pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
// initialise the vertical position controller
if (!pos_control->is_active_z()) {
@ -76,8 +78,6 @@ void ModeLand::gps_run()
// Land State Machine Determination
if (is_disarmed_or_landed()) {
make_safe_ground_handling();
loiter_nav->clear_pilot_desired_acceleration();
loiter_nav->init_target();
} else {
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);

15
ArduCopter/mode_rtl.cpp
View File

@ -273,9 +273,6 @@ void ModeRTL::descent_start()
_state = SubMode::FINAL_DESCENT;
_state_complete = false;
// Set wp navigation target to above home
loiter_nav->init_target(wp_nav->get_wp_destination().xy());
// initialise altitude target to stopping point
pos_control->init_z_controller_stopping_point();
@ -363,8 +360,14 @@ void ModeRTL::land_start()
_state = SubMode::LAND;
_state_complete = false;
// Set wp navigation target to above home
loiter_nav->init_target(wp_nav->get_wp_destination().xy());
// set horizontal speed and acceleration limits
pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
// initialise loiter target destination
if (!pos_control->is_active_xy()) {
pos_control->init_xy_controller();
}
// initialise the vertical position controller
if (!pos_control->is_active_z()) {
@ -405,8 +408,6 @@ void ModeRTL::land_run(bool disarm_on_land)
// if not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) {
make_safe_ground_handling();
loiter_nav->clear_pilot_desired_acceleration();
loiter_nav->init_target();
return;
}