dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

navigator: added parameter for acceptance radius for take-off mission items

This commit is contained in:
Julian Oes 2014-06-10 17:24:04 +02:00
parent 784041e0ab
commit fd1f1c81ef
4 changed files with 25 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/modules/navigator/mission_block.cpp
View File

@ -102,13 +102,19 @@ MissionBlock::is_mission_item_reached()
_navigator_priv->get_global_position()->alt,
&dist_xy, &dist_z);
if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
/* require only altitude for takeoff */
if (_navigator_priv->get_global_position()->alt > altitude_amsl - _mission_item.acceptance_radius) {
if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator_priv->get_vstatus()->is_rotary_wing) {
/* require only altitude for takeoff for multicopter */
if (_navigator_priv->get_global_position()->alt >
altitude_amsl - _navigator_priv->get_takeoff_acceptance_radius()) {
_waypoint_position_reached = true;
}
} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
/* for takeoff mission items use the parameter for the takeoff acceptance radius */
if (dist >= 0.0f && dist <= _navigator_priv->get_takeoff_acceptance_radius()) {
_waypoint_position_reached = true;
}
} else {
/* for normal mission items used their acceptance radius */
if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) {
_waypoint_position_reached = true;
}

2
src/modules/navigator/navigator.h
View File

@ -109,6 +109,7 @@ public:
Geofence& get_geofence() { return _geofence; }
bool get_is_in_loiter() { return _is_in_loiter; }
float get_loiter_radius() { return _param_loiter_radius.get(); }
float get_takeoff_acceptance_radius() { return _param_takeoff_acceptance_radius.get(); }
int get_mavlink_fd() { return _mavlink_fd; }
private:
@ -156,6 +157,7 @@ private:
bool _update_triplet; /**< flags if position SP triplet needs to be published */
control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */
control::BlockParamFloat _param_takeoff_acceptance_radius; /**< acceptance for takeoff */
/**
* Retrieve global position
*/

3
src/modules/navigator/navigator_main.cpp
View File

@ -122,7 +122,8 @@ Navigator::Navigator() :
_loiter(this, "LOI"),
_rtl(this, "RTL"),
_update_triplet(false),
_param_loiter_radius(this, "LOITER_RAD")
_param_loiter_radius(this, "LOITER_RAD"),
_param_takeoff_acceptance_radius(this, "TF_ACC_RAD")
{
updateParams();
}

11
src/modules/navigator/navigator_params.c
View File

@ -53,3 +53,14 @@
* @group Mission
*/
PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
/**
* Takeoff Acceptance Radius (FW only)
*
* Acceptance radius for fixedwing.
*
* @unit meters
* @min 1.0
* @group Mission
*/
PARAM_DEFINE_FLOAT(NAV_TF_ACC_RAD, 25.0f);