forked from Archive/PX4-Autopilot
Merge pull request #12 from swissfang/bottledrop
Drop at the exact timing, drop only if facing into the right direction
This commit is contained in:
commit
6aee0baa30
|
@ -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
|
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
|
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 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 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]
|
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 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);
|
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;
|
ground_distance = _global_pos.alt - _target_position.alt;
|
||||||
|
|
||||||
if (counter % 90 == 0) {
|
// Distance to drop position and angle error to approach vector
|
||||||
mavlink_log_info(_mavlink_fd, "#audio: drop distance %.2f", (double)distance_real);
|
// 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 error %u", (unsigned)distance_real, (unsigned)math::degrees(approach_error));
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
switch (_drop_state) {
|
switch (_drop_state) {
|
||||||
|
@ -608,6 +619,9 @@ BottleDrop::task_main()
|
||||||
_onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &_onboard_mission);
|
_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;
|
_drop_state = DROP_STATE_TARGET_SET;
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
@ -622,9 +636,10 @@ BottleDrop::task_main()
|
||||||
} else {
|
} else {
|
||||||
|
|
||||||
// We're close enough - open the bay
|
// 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) {
|
if (isfinite(distance_real) && distance_real < distance_open_door &&
|
||||||
|
fabsf(approach_error) < math::radians(20.0f)) {
|
||||||
open_bay();
|
open_bay();
|
||||||
_drop_state = DROP_STATE_BAY_OPEN;
|
_drop_state = DROP_STATE_BAY_OPEN;
|
||||||
mavlink_log_info(_mavlink_fd, "#audio: opening bay");
|
mavlink_log_info(_mavlink_fd, "#audio: opening bay");
|
||||||
|
@ -642,11 +657,8 @@ BottleDrop::task_main()
|
||||||
map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED);
|
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);
|
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) &&
|
if (isfinite(distance_real) &&
|
||||||
(distance_real < precision) && ((distance_real < future_distance) ||
|
(distance_real < precision) && ((distance_real < future_distance))) {
|
||||||
(distance_real < precision / 10.0f))) {
|
|
||||||
drop();
|
drop();
|
||||||
_drop_state = DROP_STATE_DROPPED;
|
_drop_state = DROP_STATE_DROPPED;
|
||||||
mavlink_log_info(_mavlink_fd, "#audio: payload dropped");
|
mavlink_log_info(_mavlink_fd, "#audio: payload dropped");
|
||||||
|
|
|
@ -92,11 +92,11 @@ PARAM_DEFINE_FLOAT(BD_PRECISION, 30.0f);
|
||||||
* http://en.wikipedia.org/wiki/Drag_coefficient
|
* http://en.wikipedia.org/wiki/Drag_coefficient
|
||||||
*
|
*
|
||||||
* @unit meter
|
* @unit meter
|
||||||
* @min 1.0
|
* @min 0.08
|
||||||
* @max 80.0
|
* @max 1.5
|
||||||
* @group Payload drop
|
* @group Payload drop
|
||||||
*/
|
*/
|
||||||
PARAM_DEFINE_FLOAT(BD_OBJ_CD, 0.86f);
|
PARAM_DEFINE_FLOAT(BD_OBJ_CD, 0.1f);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Payload mass
|
* Payload mass
|
||||||
|
|
Loading…
Reference in New Issue