calculate heading from current initial movement

PX4 · Nov 25, 2021 · 1e72047 · 1e72047
1 parent c07e18d
commit 1e72047
Show file tree

Hide file tree

Showing 3 changed files with 18 additions and 13 deletions.
diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
@@ -643,7 +643,7 @@ FixedwingPositionControl::in_takeoff_situation()
 	//        && (_current_altitude <= _takeoff_ground_alt + _param_fw_clmbout_diff.get())
 	//        && (!_autogyro_takeoff.isInitialized() || _autogyro_takeoff.climbout());
 
-    return (!_autogyro_takeoff.isInitialized() || _autogyro_takeoff.climbout());
+	return (!_autogyro_takeoff.isInitialized() || _autogyro_takeoff.climbout());
 
 	//return (_current_altitude <= _takeoff_ground_alt + _param_fw_clmbout_diff.get())
 	//       && (!_autogyro_takeoff.isInitialized() || _autogyro_takeoff.climbout());

diff --git a/src/modules/fw_pos_control_l1/autogyro_takeoff/AutogyroTakeoff.cpp b/src/modules/fw_pos_control_l1/autogyro_takeoff/AutogyroTakeoff.cpp
@@ -44,6 +44,7 @@
 #include "AutogyroTakeoff.h"
 #include <systemlib/mavlink_log.h>
 #include <mathlib/mathlib.h>
+#include <lib/geo/geo.h>
 
 #include <uORB/Publication.hpp>
 
@@ -72,8 +73,10 @@ void AutogyroTakeoff::init(const hrt_abstime &now, float yaw, double current_lat
 	_time_in_state = now;
 	_last_sent_release_status = now;
 	_climbout = true; // this is true until climbout is finished
-	_start_wp(0) = current_lat;
-	_start_wp(1) = current_lon;
+	_takeoff_wp(0) = current_lat;
+	_takeoff_wp(1) = current_lon;
+	_initial_wp(0) = current_lat;
+	_initial_wp(1) = current_lon;
 }
 
 void AutogyroTakeoff::update(const hrt_abstime &now, float airspeed, float rotor_rpm, float alt_agl,
@@ -182,6 +185,8 @@ void AutogyroTakeoff::update(const hrt_abstime &now, float airspeed, float rotor
 			}
 
 			if (ready_for_release) {
+				_init_yaw = get_bearing_to_next_waypoint(_initial_wp(0), _initial_wp(1), current_lat, current_lon);
+
 				_state = AutogyroTakeoffState::TAKEOFF_RELEASE;
 				_time_in_state = now;
 				// TODO: pripravit funkci do release
@@ -207,14 +212,11 @@ void AutogyroTakeoff::update(const hrt_abstime &now, float airspeed, float rotor
 	    OUT: Command for release
 	*/
 	case AutogyroTakeoffState::TAKEOFF_RELEASE: {
-			// Wait for ACK from platform
-			//PX4_INFO("RELEASE, agl: %f", (double) alt_agl);
-			play_release_tone();
-
 			// Waiting to get full throttle
 			if (hrt_elapsed_time(&_time_in_state) < (_param_rwto_ramp_time.get() * 1_s)) {
 				// Send release CMD
 				takeoff_information.result = 1;
+				play_release_tone();
 			}
 
 			autogyro_takeoff_status.rpm = true;
@@ -225,9 +227,10 @@ void AutogyroTakeoff::update(const hrt_abstime &now, float airspeed, float rotor
 				play_next_tone();
 				_time_in_state = now;
 
+				// set current position as center of loiter
 				if (_param_rwto_hdg.get() == 0) {
-					_start_wp(0) = current_lat;
-					_start_wp(1) = current_lon;
+					_takeoff_wp(0) = current_lat;
+					_takeoff_wp(1) = current_lon;
 				}
 			}
 		}
@@ -416,8 +419,8 @@ float AutogyroTakeoff::getThrottle(const hrt_abstime &now, float tecsThrottle)
 	case AutogyroTakeoffState::PRE_TAKEOFF_PREROTATE:
 	case AutogyroTakeoffState::PRE_TAKEOFF_DONE: {
 			if (_param_ag_prerotator_type.get() == AutogyroTakeoffType::WOPREROT_RUNWAY) {
-				float throttlea = ((now - _time_in_state) / (_param_rwto_ramp_time.get() * 1_s)) * _param_rwto_max_thr.get();
-				return math::min(throttlea, _param_rwto_max_thr.get());
+				float throttle = ((now - _time_in_state) / (_param_rwto_ramp_time.get() * 1_s)) * _param_rwto_max_thr.get();
+				return math::min(throttle, _param_rwto_max_thr.get());
 
 			} else {
 				return 0;

diff --git a/src/modules/fw_pos_control_l1/autogyro_takeoff/AutogyroTakeoff.h b/src/modules/fw_pos_control_l1/autogyro_takeoff/AutogyroTakeoff.h
@@ -119,7 +119,7 @@ class __EXPORT AutogyroTakeoff : public ModuleParams
 	float getMinPitch(float climbout_min, float min);
 	float getMaxPitch(float max);
 	// bool setState(int new_state);
-	const matrix::Vector2d &getStartWP() const { return _start_wp; };
+	const matrix::Vector2d &getStartWP() const { return _takeoff_wp; };
 
 	void reset();
 
@@ -138,7 +138,9 @@ class __EXPORT AutogyroTakeoff : public ModuleParams
 	hrt_abstime _last_sent_release_status{0};
 	float _init_yaw{0.f};
 	bool _climbout{false};
-	matrix::Vector2d _start_wp;
+
+	matrix::Vector2d _initial_wp;
+	matrix::Vector2d _takeoff_wp;
 
 	uORB::Publication<tune_control_s> _tune_control{ORB_ID(tune_control)};
 	uORB::Publication<autogyro_takeoff_status_s> _autogyro_takeoff_status_pub{ORB_ID(autogyro_takeoff_status)};