Implement altitude acceptance radius

src/modules/navigator/mission_block.cpp

@@ -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;
 			}
 		}

src/modules/navigator/navigator.h

@@ -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 */
 	

src/modules/navigator/navigator_main.cpp

@@ -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()
 {

src/modules/navigator/navigator_params.c

@@ -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
  *