fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Implement altitude acceptance radius

Browse Source
This commit is contained in:
sander 2016-07-28 20:58:48 +02:00 committed by Lorenz Meier
parent b5ef5cabb5
commit e4f20f98cd
4 changed files with 59 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -167,13 +167,13 @@ MissionBlock::is_mission_item_reached()
&& _navigator->get_vstatus()->is_rotary_wing) {
/* require only altitude for takeoff for multicopter, do not use waypoint acceptance radius */
if (_navigator->get_global_position()->alt >
altitude_amsl - _navigator->get_acceptance_radius()) {
altitude_amsl - _navigator->get_altitude_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->get_acceptance_radius()
&& dist_z <= _navigator->get_default_acceptance_radius()) {
&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
_waypoint_position_reached = true;
}
} else if (!_navigator->get_vstatus()->is_rotary_wing &&
@ -186,7 +186,7 @@ MissionBlock::is_mission_item_reached()
* radius (+ some margin). Time inside and turn count is handled elsewhere.
*/
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f)
&& dist_z <= _navigator->get_default_acceptance_radius()) {
&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
_waypoint_position_reached = true;
} else {
@ -214,7 +214,7 @@ MissionBlock::is_mission_item_reached()
&dist_xy, &dist_z);
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f)
&& dist_z <= _navigator->get_default_acceptance_radius()) {
&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
// now set the loiter to the final altitude in the NAV_CMD_LOITER_TO_ALT mission item
curr_sp->alt = altitude_amsl;
@ -223,7 +223,7 @@ MissionBlock::is_mission_item_reached()
} else {
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f)
&& dist_z <= _navigator->get_default_acceptance_radius()) {
&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
_waypoint_position_reached = true;
@ -253,7 +253,8 @@ MissionBlock::is_mission_item_reached()
}
if (dist >= 0.0f && dist <= mission_acceptance_radius
&& dist_z <= _navigator->get_default_acceptance_radius()) {
&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
printf("reached acrad: %f \n", (double)_navigator->get_altitude_acceptance_radius());
_waypoint_position_reached = true;
}
}

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

@ -166,6 +166,13 @@ public:
*/
float get_acceptance_radius();
/**
* Get the altitude acceptance radius
*
* @return the distance from the target altitude before considering the waypoint reached
*/
float get_altitude_acceptance_radius();
/**
* Get the cruising speed
*
@ -285,6 +292,8 @@ private:
control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */
control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */
control::BlockParamFloat _param_fw_alt_acceptance_radius; /**< acceptance radius for fixedwing altitude */
control::BlockParamFloat _param_mc_alt_acceptance_radius; /**< acceptance radius for multicopter altitude */
control::BlockParamInt _param_datalinkloss_act; /**< select data link loss action */
control::BlockParamInt _param_rcloss_act; /**< select data link loss action */

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

@ -155,6 +155,8 @@ Navigator::Navigator() :
_follow_target(this, "TAR"),
_param_loiter_radius(this, "LOITER_RAD"),
_param_acceptance_radius(this, "ACC_RAD"),
_param_fw_alt_acceptance_radius(this, "FW_ALT_RAD"),
_param_mc_alt_acceptance_radius(this, "MC_ALT_RAD"),
_param_datalinkloss_act(this, "DLL_ACT"),
_param_rcloss_act(this, "RCL_ACT"),
_param_cruising_speed_hover(this, "MPC_XY_CRUISE", false),
@ -723,6 +725,18 @@ Navigator::get_acceptance_radius()
return get_acceptance_radius(_param_acceptance_radius.get());
}
float
Navigator::get_altitude_acceptance_radius()
{
if (!this->get_vstatus()->is_rotary_wing) {
return _param_fw_alt_acceptance_radius.get();
} else {
return _param_mc_alt_acceptance_radius.get();
}
}
float
Navigator::get_cruising_speed()
{

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

@ -58,7 +58,7 @@ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
* Acceptance Radius
*
* Default acceptance radius, overridden by acceptance radius of waypoint if set.
* For fixed wing NAV_ACC_RAD is the vertical acceptance, as the L1 turning distance is used for horizontal acceptance.
* For fixed wing the L1 turning distance is used for horizontal acceptance.
*
* @unit m
* @min 0.05
@ -69,6 +69,34 @@ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
*/
PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 10.0f);
/**
* FW Altitude Acceptance Radius
*
* Acceptance radius for fixedwing altitude.
*
* @unit m
* @min 0.05
* @max 200.0
* @decimal 1
* @increment 0.5
* @group Mission
*/
PARAM_DEFINE_FLOAT(NAV_FW_ALT_RAD, 10.0f);
/**
* MC Altitude Acceptance Radius
*
* Acceptance radius for multicopter altitude.
*
* @unit m
* @min 0.05
* @max 200.0
* @decimal 1
* @increment 0.5
* @group Mission
*/
PARAM_DEFINE_FLOAT(NAV_MC_ALT_RAD, 3.0f);
/**
* Set data link loss failsafe mode
*