|
|
|
@ -748,14 +748,14 @@ bool Copter::verify_payload_place()
@@ -748,14 +748,14 @@ bool Copter::verify_payload_place()
|
|
|
|
|
// we're there; set loiter target
|
|
|
|
|
auto_payload_place_start(wp_nav->get_wp_destination()); |
|
|
|
|
nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start; |
|
|
|
|
// no break
|
|
|
|
|
FALLTHROUGH; |
|
|
|
|
case PayloadPlaceStateType_Calibrating_Hover_Start: |
|
|
|
|
// hover for 1 second to get an idea of what our hover
|
|
|
|
|
// throttle looks like
|
|
|
|
|
debug("Calibrate start"); |
|
|
|
|
nav_payload_place.hover_start_timestamp = now; |
|
|
|
|
nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover; |
|
|
|
|
// no break
|
|
|
|
|
FALLTHROUGH; |
|
|
|
|
case PayloadPlaceStateType_Calibrating_Hover: { |
|
|
|
|
if (now - nav_payload_place.hover_start_timestamp < hover_throttle_calibrate_time) { |
|
|
|
|
// still calibrating...
|
|
|
|
@ -768,13 +768,13 @@ bool Copter::verify_payload_place()
@@ -768,13 +768,13 @@ bool Copter::verify_payload_place()
|
|
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "hover throttle delta: %f", static_cast<double>(hover_throttle_delta)); |
|
|
|
|
nav_payload_place.state = PayloadPlaceStateType_Descending_Start; |
|
|
|
|
} |
|
|
|
|
// no break
|
|
|
|
|
FALLTHROUGH; |
|
|
|
|
case PayloadPlaceStateType_Descending_Start: |
|
|
|
|
nav_payload_place.descend_start_timestamp = now; |
|
|
|
|
nav_payload_place.descend_start_altitude = inertial_nav.get_altitude(); |
|
|
|
|
nav_payload_place.descend_throttle_level = 0; |
|
|
|
|
nav_payload_place.state = PayloadPlaceStateType_Descending; |
|
|
|
|
// no break
|
|
|
|
|
FALLTHROUGH; |
|
|
|
|
case PayloadPlaceStateType_Descending: |
|
|
|
|
// make sure we don't descend too far:
|
|
|
|
|
debug("descended: %f cm (%f cm max)", (nav_payload_place.descend_start_altitude - inertial_nav.get_altitude()), nav_payload_place.descend_max); |
|
|
|
@ -808,7 +808,7 @@ bool Copter::verify_payload_place()
@@ -808,7 +808,7 @@ bool Copter::verify_payload_place()
|
|
|
|
|
return false; |
|
|
|
|
} |
|
|
|
|
nav_payload_place.state = PayloadPlaceStateType_Releasing_Start; |
|
|
|
|
// no break
|
|
|
|
|
FALLTHROUGH; |
|
|
|
|
case PayloadPlaceStateType_Releasing_Start: |
|
|
|
|
#if GRIPPER_ENABLED == ENABLED |
|
|
|
|
if (g2.gripper.valid()) { |
|
|
|
@ -823,7 +823,7 @@ bool Copter::verify_payload_place()
@@ -823,7 +823,7 @@ bool Copter::verify_payload_place()
|
|
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "Gripper code disabled"); |
|
|
|
|
#endif |
|
|
|
|
nav_payload_place.state = PayloadPlaceStateType_Releasing; |
|
|
|
|
// no break
|
|
|
|
|
FALLTHROUGH; |
|
|
|
|
case PayloadPlaceStateType_Releasing: |
|
|
|
|
#if GRIPPER_ENABLED == ENABLED |
|
|
|
|
if (g2.gripper.valid() && !g2.gripper.released()) { |
|
|
|
@ -831,24 +831,24 @@ bool Copter::verify_payload_place()
@@ -831,24 +831,24 @@ bool Copter::verify_payload_place()
|
|
|
|
|
} |
|
|
|
|
#endif |
|
|
|
|
nav_payload_place.state = PayloadPlaceStateType_Released; |
|
|
|
|
// no break
|
|
|
|
|
FALLTHROUGH; |
|
|
|
|
case PayloadPlaceStateType_Released: { |
|
|
|
|
nav_payload_place.state = PayloadPlaceStateType_Ascending_Start; |
|
|
|
|
} |
|
|
|
|
// no break
|
|
|
|
|
FALLTHROUGH; |
|
|
|
|
case PayloadPlaceStateType_Ascending_Start: { |
|
|
|
|
Location_Class target_loc = inertial_nav.get_position(); |
|
|
|
|
target_loc.alt = nav_payload_place.descend_start_altitude; |
|
|
|
|
auto_wp_start(target_loc); |
|
|
|
|
nav_payload_place.state = PayloadPlaceStateType_Ascending; |
|
|
|
|
} |
|
|
|
|
// no break
|
|
|
|
|
FALLTHROUGH; |
|
|
|
|
case PayloadPlaceStateType_Ascending: |
|
|
|
|
if (!wp_nav->reached_wp_destination()) { |
|
|
|
|
return false; |
|
|
|
|
} |
|
|
|
|
nav_payload_place.state = PayloadPlaceStateType_Done; |
|
|
|
|
// no break
|
|
|
|
|
FALLTHROUGH; |
|
|
|
|
case PayloadPlaceStateType_Done: |
|
|
|
|
return true; |
|
|
|
|
default: |
|
|
|
|