From 960587b10ff351c2ba9dc0ef4e9b5d5b2fabfbe7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Sep 2014 11:03:32 +0200 Subject: [PATCH 1/4] Drop at the exact timing, drop only if facing into the right direction --- src/modules/bottle_drop/bottle_drop.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 0bb65252f9..56c09970d3 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -626,9 +626,15 @@ BottleDrop::task_main() } else { // We're close enough - open the bay - distance_open_door = math::max(5.0f, 3.0f * fabsf(t_door * groundspeed_body)); + distance_open_door = math::max(10.0f, 3.0f * fabsf(t_door * groundspeed_body)); - if (isfinite(distance_real) && distance_real < distance_open_door) { + float ground_direction = atan2f(_global_pos.vel_e, _global_pos.vel_n); + float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon); + + float approach_error = math::wrap_pi(ground_direction - approach_direction); + + if (isfinite(distance_real) && distance_real < distance_open_door && + approach_error < math::radians(20.0f)) { open_bay(); _drop_state = DROP_STATE_BAY_OPEN; mavlink_log_info(_mavlink_fd, "#audio: opening bay"); @@ -646,11 +652,8 @@ BottleDrop::task_main() map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED); future_distance = get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon); - warnx("Distance real: %.2f", (double)distance_real); - if (isfinite(distance_real) && - (distance_real < precision) && ((distance_real < future_distance) || - (distance_real < precision / 10.0f))) { + (distance_real < precision) && ((distance_real < future_distance))) { drop(); _drop_state = DROP_STATE_DROPPED; mavlink_log_info(_mavlink_fd, "#audio: payload dropped"); From 3834796ce94d424701e20c047b1bf29fcc4ffd61 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Sep 2014 11:39:56 +0200 Subject: [PATCH 2/4] More user feedback on approach angle and approach error. Fix check for approach error --- src/modules/bottle_drop/bottle_drop.cpp | 27 ++++++++++++++++--------- 1 file changed, 18 insertions(+), 9 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 56c09970d3..a3eb721e35 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -384,6 +384,7 @@ BottleDrop::task_main() float x_f, y_f; // to-be position of the UAV after dt_runs seconds in projected coordinates double x_f_NED, y_f_NED; // to-be position of the UAV after dt_runs seconds in NED float distance_open_door; // The distance the UAV travels during its doors open [m] + float approach_error = 0.0f; // The error in radians between current ground vector and desired ground vector float distance_real = 0; // The distance between the UAVs position and the drop point [m] float future_distance = 0; // The distance between the UAVs to-be position and the drop point [m] @@ -505,11 +506,21 @@ BottleDrop::task_main() float windspeed_norm = sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east); float groundspeed_body = sqrtf(_global_pos.vel_n * _global_pos.vel_n + _global_pos.vel_e * _global_pos.vel_e); - distance_real = fabsf(get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _drop_position.lat, _drop_position.lon)); ground_distance = _global_pos.alt - _target_position.alt; - if (counter % 90 == 0) { - mavlink_log_info(_mavlink_fd, "#audio: drop distance %.2f", (double)distance_real); + // Distance to drop position and angle error to approach vector + // are relevant in all states greater than target valid (which calculates these positions) + if (_drop_state > DROP_STATE_TARGET_VALID) { + distance_real = fabsf(get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _drop_position.lat, _drop_position.lon)); + + float ground_direction = atan2f(_global_pos.vel_e, _global_pos.vel_n); + float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon); + + approach_error = _wrap_pi(ground_direction - approach_direction); + + if (counter % 90 == 0) { + mavlink_log_critical(_mavlink_fd, "drop distance %u, heading: %u", (unsigned)distance_real, (unsigned)math::degrees(approach_error)); + } } switch (_drop_state) { @@ -612,6 +623,9 @@ BottleDrop::task_main() _onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &_onboard_mission); } + float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon); + mavlink_log_critical(_mavlink_fd, "position set, approach heading: %u", (unsigned)distance_real, (unsigned)math::degrees(approach_direction + M_PI_F)); + _drop_state = DROP_STATE_TARGET_SET; } break; @@ -628,13 +642,8 @@ BottleDrop::task_main() // We're close enough - open the bay distance_open_door = math::max(10.0f, 3.0f * fabsf(t_door * groundspeed_body)); - float ground_direction = atan2f(_global_pos.vel_e, _global_pos.vel_n); - float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon); - - float approach_error = math::wrap_pi(ground_direction - approach_direction); - if (isfinite(distance_real) && distance_real < distance_open_door && - approach_error < math::radians(20.0f)) { + fabsf(approach_error) < math::radians(20.0f)) { open_bay(); _drop_state = DROP_STATE_BAY_OPEN; mavlink_log_info(_mavlink_fd, "#audio: opening bay"); From cb11d1f99e6c8dd37a844eabdbd77d3fde2f2398 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Sep 2014 12:00:37 +0200 Subject: [PATCH 3/4] Better message formatting --- src/modules/bottle_drop/bottle_drop.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index a3eb721e35..91f1b1c950 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -519,7 +519,7 @@ BottleDrop::task_main() approach_error = _wrap_pi(ground_direction - approach_direction); if (counter % 90 == 0) { - mavlink_log_critical(_mavlink_fd, "drop distance %u, heading: %u", (unsigned)distance_real, (unsigned)math::degrees(approach_error)); + mavlink_log_critical(_mavlink_fd, "drop distance %u, heading error %u", (unsigned)distance_real, (unsigned)math::degrees(approach_error)); } } From 8296a14e9befae6b36ef04aff78c502f3c7b08a7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Sep 2014 14:36:25 +0200 Subject: [PATCH 4/4] Bottle drop: Better cd default --- src/modules/bottle_drop/bottle_drop_params.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop_params.c b/src/modules/bottle_drop/bottle_drop_params.c index e5d35bf0a2..51ebfb9a19 100644 --- a/src/modules/bottle_drop/bottle_drop_params.c +++ b/src/modules/bottle_drop/bottle_drop_params.c @@ -92,11 +92,11 @@ PARAM_DEFINE_FLOAT(BD_PRECISION, 30.0f); * http://en.wikipedia.org/wiki/Drag_coefficient * * @unit meter - * @min 1.0 - * @max 80.0 + * @min 0.08 + * @max 1.5 * @group Payload drop */ -PARAM_DEFINE_FLOAT(BD_OBJ_CD, 0.86f); +PARAM_DEFINE_FLOAT(BD_OBJ_CD, 0.1f); /** * Payload mass