fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 17:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'upstream/master' into HEAD

Browse Source 
Conflicts:
	src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
	src/modules/navigator/geofence.cpp
	src/modules/navigator/mission.cpp
This commit is contained in:
Thomas Gubler
2014-09-28 12:36:26 +02:00
parent d113fcfc54 8a18cfa386
commit 3efffb68e7
49 changed files with 2558 additions and 736 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission.cpp
+86 -2
View File
@@ -36,12 +36,14 @@
* Helper class to access missions
*
* @author Julian Oes <julian@oes.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <sys/types.h>
#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <float.h>
#include <drivers/drv_hrt.h>
@@ -49,6 +51,7 @@
#include <mavlink/mavlink_log.h>
#include <systemlib/err.h>
#include <geo/geo.h>
#include <lib/mathlib/mathlib.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
@@ -62,6 +65,7 @@ Mission::Mission(Navigator *navigator, const char *name) :
_param_onboard_enabled(this, "MIS_ONBOARD_EN", false),
_param_takeoff_alt(this, "MIS_TAKEOFF_ALT", false),
_param_dist_1wp(this, "MIS_DIST_1WP", false),
_param_altmode(this, "MIS_ALTMODE", false),
_onboard_mission({0}),
_offboard_mission({0}),
_current_onboard_mission_index(-1),
@@ -70,7 +74,11 @@ Mission::Mission(Navigator *navigator, const char *name) :
_takeoff(false),
_mission_type(MISSION_TYPE_NONE),
_inited(false),
_dist_1wp_ok(false)
_dist_1wp_ok(false),
_missionFeasiblityChecker(),
_min_current_sp_distance_xy(FLT_MAX),
_mission_item_previous_alt(NAN),
_distance_current_previous(0.0f)
{
/* load initial params */
updateParams();
@@ -142,6 +150,8 @@ Mission::on_active()
advance_mission();
set_mission_items();
} else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) {
altitude_sp_foh_update();
} else {
/* if waypoint position reached allow loiter on the setpoint */
if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) {
@@ -200,7 +210,7 @@ Mission::update_offboard_mission()
* however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing,
_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing,
dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
_navigator->get_home_position()->alt);
@@ -311,11 +321,19 @@ Mission::set_mission_items()
/* make sure param is up to date */
updateParams();
/* reset the altitude foh logic, if altitude foh is enabled (param) a new foh element starts now */
altitude_sp_foh_reset();
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
/* set previous position setpoint to current */
set_previous_pos_setpoint();
/* Copy previous mission item altitude (can be extended to a copy of the full mission item if needed) */
if (pos_sp_triplet->previous.valid) {
_mission_item_previous_alt = _mission_item.altitude;
}
/* get home distance state */
bool home_dist_ok = check_dist_1wp();
/* the home dist check provides user feedback, so we initialize it to this */
@@ -444,9 +462,75 @@ Mission::set_mission_items()
}
}
/* Save the distance between the current sp and the previous one */
if (pos_sp_triplet->current.valid && pos_sp_triplet->previous.valid) {
_distance_current_previous = get_distance_to_next_waypoint(pos_sp_triplet->current.lat,
pos_sp_triplet->current.lon,
pos_sp_triplet->previous.lat,
pos_sp_triplet->previous.lon);
}
_navigator->set_position_setpoint_triplet_updated();
}
void
Mission::altitude_sp_foh_update()
{
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
/* Don't change setpoint if last and current waypoint are not valid */
if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid ||
!isfinite(_mission_item_previous_alt)) {
return;
}
/* Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one */
if (_distance_current_previous - _mission_item.acceptance_radius < 0.0f) {
return;
}
/* Don't do FOH for landing and takeoff waypoints, the ground may be near
* and the FW controller has a custom landing logic */
if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
return;
}
/* Calculate distance to current waypoint */
float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
/* Save distance to waypoint if it is the smallest ever achieved, however make sure that
* _min_current_sp_distance_xy is never larger than the distance between the current and the previous wp */
_min_current_sp_distance_xy = math::min(math::min(d_current, _min_current_sp_distance_xy),
_distance_current_previous);
/* if the minimal distance is smaller then the acceptance radius, we should be at waypoint alt
* navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude */
if (_min_current_sp_distance_xy < _mission_item.acceptance_radius) {
pos_sp_triplet->current.alt = _mission_item.altitude;
} else {
/* update the altitude sp of the 'current' item in the sp triplet, but do not update the altitude sp
* of the mission item as it is used to check if the mission item is reached
* The setpoint is set linearly and such that the system reaches the current altitude at the acceptance
* radius around the current waypoint
**/
float delta_alt = (_mission_item.altitude - _mission_item_previous_alt);
float grad = -delta_alt/(_distance_current_previous - _mission_item.acceptance_radius);
float a = _mission_item_previous_alt - grad * _distance_current_previous;
pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy;
}
_navigator->set_position_setpoint_triplet_updated();
}
void
Mission::altitude_sp_foh_reset()
{
_min_current_sp_distance_xy = FLT_MAX;
}
bool
Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item)
{