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

Copter: Payload Place: Fix first run abort

This commit is contained in:
Leonard Hall 2024-07-15 17:42:57 +09:30 committed by Peter Barker
parent 1c54dd9e5d
commit 8260648a66
1 changed files with 12 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
ArduCopter/mode_auto.cpp
View File

@ -1255,13 +1255,21 @@ void PayloadPlace::run()
const uint32_t descent_thrust_cal_duration_ms = 2000; // milliseconds
const uint32_t placed_check_duration_ms = 500; // how long we have to be below a throttle threshold before considering placed
// Vertical thrust is taken from the attitude controller before angle boost is added
auto &g2 = copter.g2;
const auto &g = copter.g;
auto &inertial_nav = copter.inertial_nav;
auto *attitude_control = copter.attitude_control;
const auto &pos_control = copter.pos_control;
const auto &wp_nav = copter.wp_nav;
// Vertical thrust is taken from the attitude controller before angle boost is added
const float thrust_level = attitude_control->get_throttle_in();
const uint32_t now_ms = AP_HAL::millis();
// relax position target if we might be landed
// if we discover we've landed then immediately release the load:
if (copter.ap.land_complete || copter.ap.land_complete_maybe) {
pos_control->soften_for_landing_xy();
switch (state) {
case State::FlyToLocation:
// this is handled in wp_run()
@ -1289,7 +1297,9 @@ void PayloadPlace::run()
switch (state) {
case State::FlyToLocation:
case State::Descent_Start:
gcs().send_text(MAV_SEVERITY_INFO, "%s Manual release", prefix_str);
gcs().send_text(MAV_SEVERITY_INFO, "%s Abort: Gripper Open", prefix_str);
// Descent_Start has not run so we must also initalise descent_start_altitude_cm
descent_start_altitude_cm = inertial_nav.get_position_z_up_cm();
state = State::Done;
break;
case State::Descent:
@ -1307,12 +1317,6 @@ void PayloadPlace::run()
}
#endif
auto &inertial_nav = copter.inertial_nav;
auto &g2 = copter.g2;
const auto &g = copter.g;
const auto &wp_nav = copter.wp_nav;
const auto &pos_control = copter.pos_control;
switch (state) {
case State::FlyToLocation:
if (copter.wp_nav->reached_wp_destination()) {