6th batch of commit

flyingthingsintothings · Jul 4, 2024 · 5a594ba · 5a594ba
1 parent f346ebf
commit 5a594ba
Show file tree

Hide file tree

Showing 33 changed files with 271 additions and 608 deletions.
diff --git a/msg/RoverAckermannGuidanceStatus.msg b/msg/RoverAckermannGuidanceStatus.msg
diff --git a/src/modules/commander/HealthAndArmingChecks/checks/loggerCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/loggerCheck.cpp
diff --git a/src/modules/commander/HealthAndArmingChecks/checks/loggerCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/loggerCheck.hpp
diff --git a/src/modules/ekf2/EKF/imu_down_sampler/imu_down_sampler.cpp b/src/modules/ekf2/EKF/imu_down_sampler/imu_down_sampler.cpp
diff --git a/src/modules/ekf2/EKF/imu_down_sampler/imu_down_sampler.hpp b/src/modules/ekf2/EKF/imu_down_sampler/imu_down_sampler.hpp
diff --git a/src/modules/mc_pos_control/multicopter_autonomous_params.c b/src/modules/mc_pos_control/multicopter_autonomous_params.c
@@ -172,7 +172,6 @@ PARAM_DEFINE_FLOAT(MPC_YAWRAUTO_ACC, 60.f);
  * @value 2 away from home
  * @value 3 along trajectory
  * @value 4 towards waypoint (yaw first)
- * @value 5 yaw fixed
  * @group Mission
  */
 PARAM_DEFINE_INT32(MPC_YAW_MODE, 0);
diff --git a/src/modules/navigator/mission_base.h b/src/modules/navigator/mission_base.h
@@ -314,13 +314,6 @@ class MissionBase : public MissionBlock, public ModuleParams
 	 */
 	bool position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const;
 
-	/**
-	 * @brief Set the Mission Index
-	 *
-	 * @param[in] index Index of the mission item
-	 */
-	void setMissionIndex(int32_t index);
-
 	bool _is_current_planned_mission_item_valid{false};	/**< Flag indicating if the currently loaded mission item is valid*/
 	bool _mission_has_been_activated{false};		/**< Flag indicating if the mission has been activated*/
 	bool _mission_checked{false};				/**< Flag indicating if the mission has been checked by the mission validator*/
@@ -428,6 +421,13 @@ class MissionBase : public MissionBlock, public ModuleParams
 	 */
 	bool cameraWasTriggering();
 
+	/**
+	 * @brief Set the Mission Index
+	 *
+	 * @param[in] index Index of the mission item
+	 */
+	void setMissionIndex(int32_t index);
+
 	/**
 	 * @brief Parameters update
 	 *

diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c
@@ -87,7 +87,7 @@ PARAM_DEFINE_INT32(MIS_TKO_LAND_REQ, 0);
  * @increment 100
  * @group Mission
  */
-PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 10000);
+PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 900);
 
 /**
 * Enable yaw control of the mount. (Only affects multicopters and ROI mission items)

diff --git a/src/modules/navigator/rtl_mission_fast.cpp b/src/modules/navigator/rtl_mission_fast.cpp
@@ -52,27 +52,12 @@ RtlMissionFast::RtlMissionFast(Navigator *navigator) :
 
 }
 
-void RtlMissionFast::on_inactive()
-{
-	MissionBase::on_inactive();
-	_vehicle_status_sub.update();
-	_mission_index_prior_rtl = _vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION ?
-				   _mission.current_seq : -1;
-}
-
 void RtlMissionFast::on_activation()
 {
 	_home_pos_sub.update();
 
-	// set mission item to closest item if not already in mission
-	if (_mission_index_prior_rtl < 0) {
-		_is_current_planned_mission_item_valid = setMissionToClosestItem(_global_pos_sub.get().lat, _global_pos_sub.get().lon,
-				_global_pos_sub.get().alt, _home_pos_sub.get().alt, _vehicle_status_sub.get()) == PX4_OK;
-
-	} else {
-		setMissionIndex(_mission_index_prior_rtl);
-		_is_current_planned_mission_item_valid = isMissionValid();
-	}
+	_is_current_planned_mission_item_valid = setMissionToClosestItem(_global_pos_sub.get().lat, _global_pos_sub.get().lon,
+			_global_pos_sub.get().alt, _home_pos_sub.get().alt, _vehicle_status_sub.get()) == PX4_OK;
 
 	if (_land_detected_sub.get().landed) {
 		// already landed, no need to do anything, invalidad the position mission item.

diff --git a/src/modules/navigator/rtl_mission_fast.h b/src/modules/navigator/rtl_mission_fast.h
@@ -56,15 +56,12 @@ class RtlMissionFast : public RtlBase
 	~RtlMissionFast() = default;
 
 	void on_activation() override;
-	void on_inactive() override;
 
 	rtl_time_estimate_s calc_rtl_time_estimate() override;
 
 private:
 	bool setNextMissionItem() override;
 	void setActiveMissionItems() override;
 
-	int _mission_index_prior_rtl{-1};
-
 	uORB::SubscriptionData<home_position_s> _home_pos_sub{ORB_ID(home_position)};		/**< home position subscription */
 };
diff --git a/src/modules/navigator/rtl_mission_fast_reverse.cpp b/src/modules/navigator/rtl_mission_fast_reverse.cpp
@@ -52,27 +52,12 @@ RtlMissionFastReverse::RtlMissionFastReverse(Navigator *navigator) :
 
 }
 
-void RtlMissionFastReverse::on_inactive()
-{
-	MissionBase::on_inactive();
-	_vehicle_status_sub.update();
-	_mission_index_prior_rtl = _vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION ?
-				   _mission.current_seq : -1;
-}
-
 void RtlMissionFastReverse::on_activation()
 {
 	_home_pos_sub.update();
 
-	// set mission item to closest item if not already in mission. If we are in mission, set to the previous item.
-	if (_mission_index_prior_rtl < 0) {
-		_is_current_planned_mission_item_valid = setMissionToClosestItem(_global_pos_sub.get().lat, _global_pos_sub.get().lon,
-				_global_pos_sub.get().alt, _home_pos_sub.get().alt, _vehicle_status_sub.get()) == PX4_OK;
-
-	} else {
-		setMissionIndex(math::max(_mission_index_prior_rtl - 1, 0));
-		_is_current_planned_mission_item_valid = isMissionValid();
-	}
+	_is_current_planned_mission_item_valid = setMissionToClosestItem(_global_pos_sub.get().lat, _global_pos_sub.get().lon,
+			_global_pos_sub.get().alt, _home_pos_sub.get().alt, _vehicle_status_sub.get()) == PX4_OK;
 
 	if (_land_detected_sub.get().landed) {
 		// already landed, no need to do anything, invalidate the position mission item.