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

navigator: use common "acceptance radius" parameter for all modes

This commit is contained in:
Anton Babushkin 2014-06-27 11:09:49 +02:00
parent 1bae18377a
commit 52eb49ba0b
7 changed files with 15 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -100,12 +100,12 @@ MissionBlock::is_mission_item_reached()
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()) {
altitude_amsl - _navigator_priv->get_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()) {
if (dist >= 0.0f && dist <= _navigator_priv->get_acceptance_radius()) {
_waypoint_position_reached = true;
}
} else {

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

@ -115,7 +115,7 @@ public:
Geofence& get_geofence() { return _geofence; }
bool get_can_loiter_at_sp() { return _can_loiter_at_sp; }
float get_loiter_radius() { return _param_loiter_radius.get(); }
float get_takeoff_acceptance_radius() { return _param_takeoff_acceptance_radius.get(); }
float get_acceptance_radius() { return _param_acceptance_radius.get(); }
int get_mavlink_fd() { return _mavlink_fd; }
private:
@ -165,7 +165,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 */
control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */
/**
* Retrieve global position
*/

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

@ -123,7 +123,7 @@ Navigator::Navigator() :
_rtl(this, "RTL"),
_update_triplet(false),
_param_loiter_radius(this, "LOITER_RAD"),
_param_takeoff_acceptance_radius(this, "TF_ACC_RAD")
_param_acceptance_radius(this, "ACC_RAD")
{
/* Create a list of our possible navigation types */
_navigation_mode_array[0] = &_mission;

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

@ -55,12 +55,12 @@
PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
/**
* Takeoff Acceptance Radius (FW only)
* Acceptance Radius
*
* Acceptance radius for fixedwing.
* Default acceptance radius, overridden by acceptance radius of waypoint if set.
*
* @unit meters
* @min 1.0
* @group Mission
*/
PARAM_DEFINE_FLOAT(NAV_TF_ACC_RAD, 25.0f);
PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 25.0f);

15
src/modules/navigator/rtl.cpp
View File

@ -61,8 +61,7 @@ RTL::RTL(Navigator *navigator, const char *name) :
_rtl_state(RTL_STATE_NONE),
_param_return_alt(this, "RETURN_ALT"),
_param_descend_alt(this, "DESCEND_ALT"),
_param_land_delay(this, "LAND_DELAY"),
_param_acceptance_radius(this, "ACCEPT_RAD")
_param_land_delay(this, "LAND_DELAY")
{
/* load initial params */
updateParams();
@ -147,7 +146,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _param_acceptance_radius.get();
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
@ -178,7 +177,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _param_acceptance_radius.get();
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
@ -198,7 +197,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
_mission_item.acceptance_radius = _param_acceptance_radius.get();
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = false;
@ -220,7 +219,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
_mission_item.acceptance_radius = _param_acceptance_radius.get();
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = autoland;
@ -246,7 +245,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_LAND;
_mission_item.acceptance_radius = _param_acceptance_radius.get();
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
@ -265,7 +264,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_IDLE;
_mission_item.acceptance_radius = _param_acceptance_radius.get();
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;

1
src/modules/navigator/rtl.h
View File

@ -105,7 +105,6 @@ private:
control::BlockParamFloat _param_return_alt;
control::BlockParamFloat _param_descend_alt;
control::BlockParamFloat _param_land_delay;
control::BlockParamFloat _param_acceptance_radius;
};
#endif

12
src/modules/navigator/rtl_params.c
View File

@ -96,15 +96,3 @@ PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20);
* @group RTL
*/
PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f);
/**
* RTL acceptance radius
*
* Acceptance radius for waypoints set for RTL
*
* @unit meters
* @min 1
* @max
* @group RTL
*/
PARAM_DEFINE_FLOAT(RTL_ACCEPT_RAD, 25.0f);