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

Copter: Fix Payload Place Bug

This commit is contained in:
Leonard Hall 2022-07-28 11:01:08 +09:30 committed by Peter Barker
parent 022ae00199
commit 1e87b4b09a
1 changed files with 6 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduCopter/mode_auto.cpp
View File

@ -1960,6 +1960,8 @@ bool ModeAuto::verify_payload_place()
nav_payload_place.state = PayloadPlaceStateType_Releasing_Start; nav_payload_place.state = PayloadPlaceStateType_Releasing_Start;
FALLTHROUGH; FALLTHROUGH;
case PayloadPlaceStateType_Releasing_Start: case PayloadPlaceStateType_Releasing_Start:
// Reinitialise vertical position controller to remove discontinuity due to touch down of payload
pos_control->init_z_controller_no_descent();
#if GRIPPER_ENABLED == ENABLED #if GRIPPER_ENABLED == ENABLED
if (g2.gripper.valid()) { if (g2.gripper.valid()) {
gcs().send_text(MAV_SEVERITY_INFO, "PayloadPlace: Releasing the gripper"); gcs().send_text(MAV_SEVERITY_INFO, "PayloadPlace: Releasing the gripper");
@ -1967,13 +1969,13 @@ bool ModeAuto::verify_payload_place()
} else { } else {
gcs().send_text(MAV_SEVERITY_INFO, "PayloadPlace: Gripper not valid"); gcs().send_text(MAV_SEVERITY_INFO, "PayloadPlace: Gripper not valid");
nav_payload_place.state = PayloadPlaceStateType_Ascending_Start; nav_payload_place.state = PayloadPlaceStateType_Ascending_Start;
break; return false;
} }
#else #else
gcs().send_text(MAV_SEVERITY_INFO, "Gripper code disabled"); gcs().send_text(MAV_SEVERITY_INFO, "Gripper code disabled");
nav_payload_place.state = PayloadPlaceStateType_Ascending_Start;
return false;
#endif #endif
// Reinitialise vertical position controller to remove discontinuity due to touch down of payload
pos_control->init_z_controller_no_descent();
nav_payload_place.state = PayloadPlaceStateType_Releasing; nav_payload_place.state = PayloadPlaceStateType_Releasing;
FALLTHROUGH; FALLTHROUGH;
case PayloadPlaceStateType_Releasing: case PayloadPlaceStateType_Releasing:
@ -1989,7 +1991,7 @@ bool ModeAuto::verify_payload_place()
} }
FALLTHROUGH; FALLTHROUGH;
case PayloadPlaceStateType_Ascending_Start: { case PayloadPlaceStateType_Ascending_Start: {
auto_takeoff_start(inertial_nav.get_position_z_up_cm(), false); auto_takeoff_start(nav_payload_place.descend_start_altitude, false);
nav_payload_place.state = PayloadPlaceStateType_Ascending; nav_payload_place.state = PayloadPlaceStateType_Ascending;
} }
FALLTHROUGH; FALLTHROUGH;