zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Copter: Payload Place: Improve touchdown test

master_rangefinder
Leonard Hall 2 years ago committed by Randy Mackay
parent
4b20a2d5f1
commit
57e9e599f4
1 changed files with 26 additions and 28 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 54
      ArduCopter/mode_auto.cpp

54
ArduCopter/mode_auto.cpp
Unescape View File

@ -1220,7 +1220,7 @@ bool ModeAuto::payload_place_run_should_run()
return false; return false;
} }
// must not be landed // must not be landed
if (copter.ap.land_complete) { if (copter.ap.land_complete && (nav_payload_place.state == PayloadPlaceStateType_FlyToLocation || nav_payload_place.state == PayloadPlaceStateType_Calibrating_Hover_Start)) {
return false; return false;
} }
// interlock must be enabled (i.e. unsafe) // interlock must be enabled (i.e. unsafe)
@ -1881,6 +1881,7 @@ bool ModeAuto::verify_payload_place()
{ {
const uint16_t hover_throttle_calibrate_time = 2000; // milliseconds const uint16_t hover_throttle_calibrate_time = 2000; // milliseconds
const uint16_t descend_throttle_calibrate_time = 2000; // milliseconds const uint16_t descend_throttle_calibrate_time = 2000; // milliseconds
const uint16_t measure_time = 1000; // milliseconds
const float hover_throttle_placed_fraction = 0.7; // i.e. if throttle is less than 70% of hover we have placed const float hover_throttle_placed_fraction = 0.7; // i.e. if throttle is less than 70% of hover we have placed
const float descent_throttle_placed_fraction = 0.9; // i.e. if throttle is less than 90% of descent throttle we have placed const float descent_throttle_placed_fraction = 0.9; // i.e. if throttle is less than 90% of descent throttle we have placed
const uint16_t placed_time = 500; // how long we have to be below a throttle threshold before considering placed const uint16_t placed_time = 500; // how long we have to be below a throttle threshold before considering placed
@ -1889,7 +1890,7 @@ bool ModeAuto::verify_payload_place()
const uint32_t now = AP_HAL::millis(); const uint32_t now = AP_HAL::millis();
// if we discover we've landed then immediately release the load: // if we discover we've landed then immediately release the load:
if (copter.ap.land_complete) { if (copter.ap.land_complete || copter.ap.land_complete_maybe) {
switch (nav_payload_place.state) { switch (nav_payload_place.state) {
case PayloadPlaceStateType_FlyToLocation: case PayloadPlaceStateType_FlyToLocation:
case PayloadPlaceStateType_Calibrating_Hover_Start: case PayloadPlaceStateType_Calibrating_Hover_Start:
@ -1917,21 +1918,23 @@ bool ModeAuto::verify_payload_place()
payload_place_start(); payload_place_start();
return false; return false;
case PayloadPlaceStateType_Calibrating_Hover_Start: case PayloadPlaceStateType_Calibrating_Hover_Start:
// hover for 1 second to get an idea of what our hover // hover for 2 seconds to measure hover thrust
// throttle looks like
debug("Calibrate start"); debug("Calibrate start");
nav_payload_place.hover_start_timestamp = now; nav_payload_place.hover_start_timestamp = now;
nav_payload_place.hover_throttle_level = 1.0;
nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover; nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover;
FALLTHROUGH; FALLTHROUGH;
case PayloadPlaceStateType_Calibrating_Hover: { case PayloadPlaceStateType_Calibrating_Hover: {
if (now - nav_payload_place.hover_start_timestamp < hover_throttle_calibrate_time - measure_time) {
// waiting for aircraft to settle
return false;
}
if (now - nav_payload_place.hover_start_timestamp < hover_throttle_calibrate_time) { if (now - nav_payload_place.hover_start_timestamp < hover_throttle_calibrate_time) {
// still calibrating... // measure hover thrust
debug("Calibrate Timer: %d", now - nav_payload_place.hover_start_timestamp); nav_payload_place.hover_throttle_level = MIN(nav_payload_place.hover_throttle_level, current_throttle_level);
return false; return false;
} }
// we have a valid calibration. Hopefully. // hover thrust has been measured
// todo: Add filter as single sample is vulnerable to noise
nav_payload_place.hover_throttle_level = current_throttle_level;
gcs().send_text(MAV_SEVERITY_INFO, "PayloadPlace: payload hover throttle: %f", static_cast<double>(nav_payload_place.hover_throttle_level)); gcs().send_text(MAV_SEVERITY_INFO, "PayloadPlace: payload hover throttle: %f", static_cast<double>(nav_payload_place.hover_throttle_level));
nav_payload_place.state = PayloadPlaceStateType_Descending_Start; nav_payload_place.state = PayloadPlaceStateType_Descending_Start;
} }
@ -1939,11 +1942,11 @@ bool ModeAuto::verify_payload_place()
case PayloadPlaceStateType_Descending_Start: case PayloadPlaceStateType_Descending_Start:
nav_payload_place.descend_start_timestamp = now; nav_payload_place.descend_start_timestamp = now;
nav_payload_place.descend_start_altitude = inertial_nav.get_position_z_up_cm(); nav_payload_place.descend_start_altitude = inertial_nav.get_position_z_up_cm();
nav_payload_place.descend_throttle_level = 0; nav_payload_place.descend_throttle_level = 1.0;
nav_payload_place.state = PayloadPlaceStateType_Descending; nav_payload_place.state = PayloadPlaceStateType_Descending;
FALLTHROUGH; FALLTHROUGH;
case PayloadPlaceStateType_Descending: case PayloadPlaceStateType_Descending:
// make sure we don't descend too far: // make sure we don't descend too far
debug("descended: %f cm (%f cm max)", (nav_payload_place.descend_start_altitude - inertial_nav.get_position_z_up_cm()), nav_payload_place.descend_max); debug("descended: %f cm (%f cm max)", (nav_payload_place.descend_start_altitude - inertial_nav.get_position_z_up_cm()), nav_payload_place.descend_max);
if (!is_zero(nav_payload_place.descend_max) && if (!is_zero(nav_payload_place.descend_max) &&
nav_payload_place.descend_start_altitude - inertial_nav.get_position_z_up_cm() > nav_payload_place.descend_max) { nav_payload_place.descend_start_altitude - inertial_nav.get_position_z_up_cm() > nav_payload_place.descend_max) {
@ -1951,27 +1954,22 @@ bool ModeAuto::verify_payload_place()
gcs().send_text(MAV_SEVERITY_WARNING, "PayloadPlace: Reached maximum descent"); gcs().send_text(MAV_SEVERITY_WARNING, "PayloadPlace: Reached maximum descent");
return false; // we'll do any cleanups required next time through the loop return false; // we'll do any cleanups required next time through the loop
} }
// see if we've been descending long enough to calibrate a descend-throttle-level: // if the aircraft has been descending long enough, calibrate the decent thrust
if (is_zero(nav_payload_place.descend_throttle_level) && if ((now - nav_payload_place.descend_start_timestamp > descend_throttle_calibrate_time - measure_time) &&
now - nav_payload_place.descend_start_timestamp > descend_throttle_calibrate_time) { (now - nav_payload_place.descend_start_timestamp < descend_throttle_calibrate_time)) {
// todo: Add filter as single sample is vulnerable to noise // measure decent thrust
nav_payload_place.descend_throttle_level = current_throttle_level; nav_payload_place.descend_throttle_level = MIN(nav_payload_place.descend_throttle_level, current_throttle_level);
} }
// watch the throttle to determine whether the load has been placed // check for reduced thrust requirement indicating possible payload touch down
// debug("hover ratio: %f descend ratio: %f\n", current_throttle_level/nav_payload_place.hover_throttle_level, ((nav_payload_place.descend_throttle_level == 0) ? -1.0f : current_throttle_level/nav_payload_place.descend_throttle_level)); if (current_throttle_level > hover_throttle_placed_fraction * nav_payload_place.hover_throttle_level &&
if (current_throttle_level/nav_payload_place.hover_throttle_level > hover_throttle_placed_fraction && (now - nav_payload_place.descend_start_timestamp < descend_throttle_calibrate_time ||
(is_zero(nav_payload_place.descend_throttle_level) || current_throttle_level > descent_throttle_placed_fraction * nav_payload_place.descend_throttle_level)) {
current_throttle_level/nav_payload_place.descend_throttle_level > descent_throttle_placed_fraction)) {
// throttle is above both threshold ratios (or above hover threshold ration and descent threshold ratio not yet valid) // throttle is above both threshold ratios (or above hover threshold ration and descent threshold ratio not yet valid)
nav_payload_place.place_start_timestamp = 0;
return false;
}
if (nav_payload_place.place_start_timestamp == 0) {
// we've only just now hit the correct throttle level
nav_payload_place.place_start_timestamp = now; nav_payload_place.place_start_timestamp = now;
return false; return false;
} else if (now - nav_payload_place.place_start_timestamp < placed_time) { }
// keep going down.... if (now - nav_payload_place.place_start_timestamp < placed_time) {
// continue decent
debug("Place Timer: %d", now - nav_payload_place.place_start_timestamp); debug("Place Timer: %d", now - nav_payload_place.place_start_timestamp);
return false; return false;
} }

Write Preview
Loading…
Cancel
Save