diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/modules/bottle_drop/bottle_drop.cpp | 108 |
1 files changed, 68 insertions, 40 deletions
diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 5b7f7bd7f..81d1238cf 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -134,11 +134,15 @@ private: enum DROP_STATE { DROP_STATE_INIT = 0, DROP_STATE_TARGET_VALID, + DROP_STATE_TARGET_SET, DROP_STATE_BAY_OPEN, DROP_STATE_DROPPED, DROP_STATE_BAY_CLOSED } _drop_state; + struct mission_s _onboard_mission; + orb_advert_t _onboard_mission_pub; + void task_main(); void handle_command(struct vehicle_command_s *cmd); @@ -179,7 +183,9 @@ BottleDrop::BottleDrop() : _alt_clearance(70.0f), _target_position {}, _drop_position {}, - _drop_state(DROP_STATE_INIT) + _drop_state(DROP_STATE_INIT), + _onboard_mission {}, + _onboard_mission_pub(-1) { } @@ -381,10 +387,6 @@ BottleDrop::task_main() 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] - // states - bool state_drop = false; // Drop occurred = true, Drop din't occur = false - bool state_run = false; // A drop was attempted = true, the drop is still in progress = false - unsigned counter = 0; param_t param_gproperties = param_find("BD_GPROPERTIES"); @@ -415,10 +417,6 @@ BottleDrop::task_main() flight_vector_e.autocontinue = true; flight_vector_s.altitude_is_relative = false; - struct mission_s onboard_mission; - memset(&onboard_mission, 0, sizeof(onboard_mission)); - orb_advert_t onboard_mission_pub = -1; - struct wind_estimate_s wind; /* wakeup source(s) */ @@ -622,35 +620,37 @@ BottleDrop::task_main() warnx("ERROR: could not save onboard WP"); } - onboard_mission.count = 2; + _onboard_mission.count = 2; + _onboard_mission.current_seq = 0; - if (state_run && !state_drop) { - onboard_mission.current_seq = 0; + if (_onboard_mission_pub > 0) { + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); } else { - onboard_mission.current_seq = -1; + _onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &_onboard_mission); } - if (onboard_mission_pub > 0) { - orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); + _drop_state = DROP_STATE_TARGET_SET; + } + break; - } else { - onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &onboard_mission); - } + case DROP_STATE_TARGET_SET: + { + float distance_wp2 = get_distance_to_next_waypoint(flight_vector_e.lat, flight_vector_e.lon, _drop_position.lat, _drop_position.lon); - // We're close enough - open the bay - distance_open_door = math::max(5.0f, 3.0f * fabsf(t_door * groundspeed_body)); + if (distance_wp2 < distance_real) { + _onboard_mission.current_seq = 0; + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); + } else { - if (counter % 10 == 0) - warnx("dist real: %.2f, distance_open_door: %.2f, angle to wind: %.2f", - (double)distance_real, - (double)distance_open_door, - (double)(_wrap_pi(_global_pos.yaw - atan2f(wind.windspeed_north, wind.windspeed_east)))); + // We're close enough - open the bay + distance_open_door = math::max(5.0f, 3.0f * fabsf(t_door * groundspeed_body)); - if (isfinite(distance_real) && distance_real < distance_open_door) { - open_bay(); - _drop_state = DROP_STATE_BAY_OPEN; - mavlink_log_info(_mavlink_fd, "#audio: opening bay"); + if (isfinite(distance_real) && distance_real < distance_open_door) { + open_bay(); + _drop_state = DROP_STATE_BAY_OPEN; + mavlink_log_info(_mavlink_fd, "#audio: opening bay"); + } } } break; @@ -672,6 +672,14 @@ BottleDrop::task_main() drop(); _drop_state = DROP_STATE_DROPPED; mavlink_log_info(_mavlink_fd, "#audio: payload dropped"); + } else { + + float distance_wp2 = get_distance_to_next_waypoint(flight_vector_e.lat, flight_vector_e.lon, _drop_position.lat, _drop_position.lon); + + if (distance_wp2 < distance_real) { + _onboard_mission.current_seq = 0; + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); + } } } } @@ -685,6 +693,10 @@ BottleDrop::task_main() lock_release(); close_bay(); mavlink_log_info(_mavlink_fd, "#audio: closing bay"); + + // remove onboard mission + _onboard_mission.current_seq = -1; + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); } break; } @@ -767,20 +779,36 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) break; case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY: - switch ((int)(cmd->param1 + 0.5f)) { - case 0: - _drop_approval = false; - break; - case 1: - _drop_approval = true; - mavlink_log_info(_mavlink_fd, "#audio: got drop approval"); - break; + if (cmd->param1 < 0) { - default: + // Clear internal states _drop_approval = false; - break; - // XXX handle other values + _drop_state = DROP_STATE_INIT; + + // Abort if mission is present + _onboard_mission.current_seq = -1; + + if (_onboard_mission_pub > 0) { + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); + } + + } else { + switch ((int)(cmd->param1 + 0.5f)) { + case 0: + _drop_approval = false; + break; + + case 1: + _drop_approval = true; + mavlink_log_info(_mavlink_fd, "#audio: got drop approval"); + break; + + default: + _drop_approval = false; + break; + // XXX handle other values + } } answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); |