Remove soaring related messages

msg/CMakeLists.txt

@@ -206,12 +206,6 @@ set(msg_files
 	SensorsStatusImu.msg
 	SensorUwb.msg
 	SensorAirflow.msg
-	soaring_controller_heartbeat.msg
-	soaring_controller_position_setpoint.msg
-	soaring_controller_position.msg
-	soaring_controller_status.msg
-	soaring_controller_wind.msg
-	soaring_estimator_shear.msg
 	SystemPower.msg
 	TakeoffStatus.msg
 	TaskStackInfo.msg

msg/soaring_controller_heartbeat.msg

@@ -1,5 +0,0 @@
-# SOARING CONTROLLER STATE, SERVES AS A HEARTBEAT IF CONTROLLER IS STILL PUBLISHING
-
-uint64 timestamp			# time since system start (microseconds)
-
-uint64 heartbeat            # time since system start (microseconds) of the last run of soaring controller

msg/soaring_controller_position.msg

@@ -1,8 +0,0 @@
-# SOARING CONTROLLER POSITION IN ENU SOARING FRAME
-
-uint64 timestamp			# time since system start (microseconds)
-
-float32[3] pos                            # POSITION VECTOR IN SOARING ENU FRAME
-float32[3] vel                            # VELOCITY VECTOR IN SOARING ENU FRAME
-float32[3] acc                            # ACCELERATION VECTOR IN SOARING ENU FRAME
-float32[4] att                            # UNIT QUATERNION DESCRIBING BODY FRAME POSE TO ENU

msg/soaring_controller_position_setpoint.msg

@@ -1,12 +0,0 @@
-# SOARING CONTROLLER POSITION SETPOINT
-
-uint64 timestamp			# time since system start (microseconds)
-
-float32[3] pos                             # position in ENU frame
-float32[3] vel                             # velocity in ENU frame
-float32[3] acc                             # acceleration in ENU frame
-float32[3] f_command                       # force command inside controller
-float32[3] m_command                       # moment command inside controller
-float32[3] w_err                           # rotation vector to target pose
-
-

msg/soaring_controller_status.msg

@@ -1,6 +0,0 @@
-# SOARING CONTROLLER STATE, SERVES AS FLAG IF CONTROLLER IS STILL PUBLISHING
-
-uint64 timestamp			# time since system start (microseconds)
-
-bool soaring_controller_running     # if true, the soaring controller is expected to publish
-bool timeout_detected               # if true, the soaring controller has crashed

msg/soaring_controller_wind.msg

@@ -1,11 +0,0 @@
-# SOARING CONTROLLER WIND ESTIMATE, USED FOR INDI CONTROL
-
-uint64 timestamp			# time since system start (microseconds)
-
-float32[3] wind_estimate                            # WIND ESTIMATE IN ENU FRAME
-float32[3] wind_estimate_filtered                   # LP-FILTERED WIND ESTIMATE IN ENU FRAME, USED BY INDI CONTROLLER
-float32[3] position                                 # position of the current estimate in the soaring frame
-float32    airspeed                                 # current airspeed
-
-bool valid                                          # tell, if estimate is valid for shear estimator
-bool lock_params                                    # tell, if the shear estimator shall lock shear param values for soaring

msg/soaring_estimator_shear.msg

@@ -1,30 +0,0 @@
-# SOARING ESTIMATOR WIND ESTIMATE, USED FOR SELECTING THE CORRECT TRAJECTORY
-
-uint64 timestamp			# time since system start (microseconds)
-
-float32 vx                  # maximum wind in x-direction
-float32 vy                  # maximum wind in y-direction
-float32 bx                  # wind offset in x-direction
-float32 by                  # wind offset in y-direction
-float32 h                   # vertical position of shear layer in soaring frame
-float32 a                   # shear strength
-
-float32 sigma_vx            # covariance of vx
-float32 sigma_vy            # covariance of vy
-float32 sigma_bx            # covariance of bx
-float32 sigma_by            # covariance of by
-float32 sigma_h             # covariance of h
-float32 sigma_a             # covariance of a
-
-float32 coeff_0             # offset vertical wind
-float32 coeff_1             # linear coeff vertical wind
-float32 coeff_2             # quadratic coeff vertical wind
-
-float32 v_max               # discrete value of v_max (shear velocity), identifier for appropriate trajectrory
-float32 alpha               # discrete value of alpha (shear strength), identifier for appropriate trajectrory
-float32 h_ref               # discrete value of h_ref (shear location)
-float32 psi                 # discrete value of shear heading
-float32 aspd                # airspeed identifier for appropriate trajectrory
-
-bool soaring_feasible       # plausibility check
-uint64 reset_counter        # filter reset counter

src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp

@@ -340,38 +340,6 @@ FixedwingPositionINDIControl::actuator_controls_poll()
 	_actuator_controls_sub.update(&_actuators);
 }
 
-void
-FixedwingPositionINDIControl::soaring_controller_status_poll()
-{
-	if (_soaring_controller_status_sub.update(&_soaring_controller_status)) {
-		if (!_soaring_controller_status.soaring_controller_running) {
-			//PX4_INFO("Soaring controller turned off");
-		}
-
-		if (_soaring_controller_status.timeout_detected) {
-			//PX4_INFO("Controller timeout detected");
-		}
-	}
-}
-
-void
-FixedwingPositionINDIControl::soaring_estimator_shear_poll()
-{
-	if (!_switch_origin_hardcoded) {
-		if (_soaring_estimator_shear_sub.update(&_soaring_estimator_shear)) {
-			// update the shear estimate, only if we are flying in manual feedthrough for safety reasons
-			_shear_v_max = _soaring_estimator_shear.v_max;
-			_shear_alpha = _soaring_estimator_shear.alpha;
-			_shear_h_ref = _soaring_estimator_shear.h_ref;
-			_shear_heading = _soaring_estimator_shear.psi - M_PI_2_F;
-			_soaring_feasible =  _soaring_estimator_shear.soaring_feasible;
-			// the initial speed of the target trajectory can safely be updated during soaring :)
-			_shear_aspd = _soaring_estimator_shear.aspd;
-		}
-
-	}
-}
-
 void
 FixedwingPositionINDIControl::_compute_trajectory_transform()
 {
@@ -814,10 +782,6 @@ FixedwingPositionINDIControl::Run()
 		vehicle_attitude_poll();
 		vehicle_acceleration_poll();
 		vehicle_angular_velocity_poll();
-		soaring_controller_status_poll();
-
-		// update the shear estimate, only target airspeed is updated in soaring mode
-		soaring_estimator_shear_poll();
 
 		// update transform from trajectory frame to ENU frame (soaring frame)
 		_compute_trajectory_transform();
@@ -901,35 +865,6 @@ FixedwingPositionINDIControl::Run()
 		// Publish actuator controls only once in OFFBOARD
 		if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) {
 
-			// ========================================
-			// publish controller position in ENU frame
-			// ========================================
-			_soaring_controller_position.timestamp = hrt_absolute_time();
-
-			for (int i = 0; i < 3; i++) {
-				_soaring_controller_position.pos[i] = _pos(i);
-				_soaring_controller_position.vel[i] = _vel(i);
-				_soaring_controller_position.acc[i] = _acc(i);
-			}
-
-			_soaring_controller_position_pub.publish(_soaring_controller_position);
-
-			// ====================================
-			// publish controller position setpoint
-			// ====================================
-			_soaring_controller_position_setpoint.timestamp = hrt_absolute_time();
-
-			for (int i = 0; i < 3; i++) {
-				_soaring_controller_position_setpoint.pos[i] = pos_ref(i);
-				_soaring_controller_position_setpoint.vel[i] = vel_ref(i);
-				_soaring_controller_position_setpoint.acc[i] = acc_ref(i);
-				_soaring_controller_position_setpoint.f_command[i] = _f_command(i);
-				_soaring_controller_position_setpoint.m_command[i] = _m_command(i);
-				_soaring_controller_position_setpoint.w_err[i] = _w_err(i);
-			}
-
-			_soaring_controller_position_setpoint_pub.publish(_soaring_controller_position_setpoint);
-
 			// =====================
 			// publish control input
 			// =====================
@@ -973,46 +908,6 @@ FixedwingPositionINDIControl::Run()
 			_torque_sp_pub.publish(_actuators);
 			_thrust_sp.xyz[0] = _thrust;
 			_thrust_sp_pub.publish(_thrust_sp);
-			//print_message(_actuators);
-
-			// =====================
-			// publish wind estimate
-			// =====================
-			//_soaring_controller_wind = {};
-			_soaring_controller_wind.timestamp = hrt_absolute_time();
-			_soaring_controller_wind.wind_estimate[0] = wind(0);
-			_soaring_controller_wind.wind_estimate[1] = wind(1);
-			_soaring_controller_wind.wind_estimate[2] = wind(2);
-			_soaring_controller_wind.wind_estimate_filtered[0] = _wind_estimate_EKF(0);
-			_soaring_controller_wind.wind_estimate_filtered[1] = _wind_estimate_EKF(1);
-			_soaring_controller_wind.wind_estimate_filtered[2] = _wind_estimate_EKF(2);
-			_soaring_controller_wind.position[0] = _pos(0);
-			_soaring_controller_wind.position[1] = _pos(1);
-			_soaring_controller_wind.position[2] = _pos(2);
-			_soaring_controller_wind.airspeed = _true_airspeed;
-
-			if (_switch_cl_soaring) {
-				// always update shear params in closed loop soaring mode
-				_soaring_controller_wind.lock_params = false;
-
-			} else {
-				// only update in manual feedthrough in open loop soaring
-				_soaring_controller_wind.lock_params = !_switch_manual;
-			}
-
-			//Eulerf e(Quatf(_attitude.q));
-			//float bank = e(0);
-			// only declare wind estimate valid for shear estimator, if we are close to the soaring center
-			if ((float)sqrtf(powf(_pos(0), 2) + powf(_pos(1), 2)) < 100.f) {
-				_soaring_controller_wind.valid = true;
-
-			} else {
-				_soaring_controller_wind.valid = false;
-			}
-
-			_soaring_controller_wind_pub.publish(_soaring_controller_wind);
-
-
 
 			if (_counter == 100) {
 				_counter = 0;
@@ -1035,14 +930,6 @@ FixedwingPositionINDIControl::Run()
 		rate_ctrl_status.yawspeed_integ = 0.0f;
 		_rate_ctrl_status_pub.publish(rate_ctrl_status);
 
-		// ==============================
-		// publish soaring control status
-		// ==============================
-		//_soaring_controller_heartbeat_s _soaring_controller_heartbeat{};
-		_soaring_controller_heartbeat.timestamp = hrt_absolute_time();
-		_soaring_controller_heartbeat.heartbeat = hrt_absolute_time();
-		_soaring_controller_heartbeat_pub.publish(_soaring_controller_heartbeat);
-
 		// ====================
 		// publish debug values
 		// ====================

src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp

@@ -63,12 +63,6 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_thrust_setpoint.h>
 #include <uORB/topics/vehicle_torque_setpoint.h>
-#include <uORB/topics/soaring_controller_heartbeat.h>
-#include <uORB/topics/soaring_controller_position_setpoint.h>
-#include <uORB/topics/soaring_controller_position.h>
-#include <uORB/topics/soaring_controller_status.h>
-#include <uORB/topics/soaring_controller_wind.h>
-#include <uORB/topics/soaring_estimator_shear.h>
 #include <uORB/topics/offboard_control_mode.h>
 #include <uORB/topics/debug_value.h>
 #include <uORB/topics/wind.h>
@@ -124,8 +118,6 @@ private:
 	uORB::Subscription _home_position_sub{ORB_ID(home_position)};						// home position
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};                 // vehicle attitude
 //     uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)}; // vehicle body accel
-	uORB::Subscription _soaring_controller_status_sub{ORB_ID(soaring_controller_status)};			// vehicle status flags
-	uORB::Subscription _soaring_estimator_shear_sub{ORB_ID(soaring_estimator_shear)};	// shear params for trajectory selection
 	uORB::Subscription _actuator_controls_sub{ORB_ID(vehicle_torque_setpoint)};
 	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::Subscription _rc_channels_sub{ORB_ID(rc_channels)};
@@ -137,10 +129,6 @@ private:
 	uORB::Publication<vehicle_rates_setpoint_s>						_angular_vel_sp_pub{ORB_ID(vehicle_rates_setpoint)};
 	uORB::Publication<vehicle_angular_acceleration_setpoint_s>		_angular_accel_sp_pub{ORB_ID(vehicle_angular_acceleration_setpoint)};
 	uORB::PublicationMulti<rate_ctrl_status_s>						_rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)};
-	uORB::Publication<soaring_controller_heartbeat_s>				_soaring_controller_heartbeat_pub{ORB_ID(soaring_controller_status)};
-	uORB::Publication<soaring_controller_position_setpoint_s>		_soaring_controller_position_setpoint_pub{ORB_ID(soaring_controller_position_setpoint)};
-	uORB::Publication<soaring_controller_position_s>				_soaring_controller_position_pub{ORB_ID(soaring_controller_position)};
-	uORB::Publication<soaring_controller_wind_s>					_soaring_controller_wind_pub{ORB_ID(soaring_controller_wind)};
 	uORB::Publication<offboard_control_mode_s>						_offboard_control_mode_pub{ORB_ID(offboard_control_mode)};
 	uORB::Publication<debug_value_s>								_debug_value_pub{ORB_ID(debug_value)};
 
@@ -162,12 +150,6 @@ private:
 	vehicle_control_mode_s		_control_mode {};		///< control mode
 	offboard_control_mode_s		_offboard_control_mode {};	///< offboard control mode
 	vehicle_status_s		    _vehicle_status {};		///< vehicle status
-	soaring_controller_status_s	_soaring_controller_status {};	///< soaring controller status
-	soaring_controller_heartbeat_s	_soaring_controller_heartbeat{};	///< soaring controller hrt
-	soaring_controller_position_setpoint_s	_soaring_controller_position_setpoint{};	///< soaring controller pos setpoint
-	soaring_controller_position_s	_soaring_controller_position{};	///< soaring controller pos
-	soaring_controller_wind_s	_soaring_controller_wind{};	///< soaring controller wind
-	soaring_estimator_shear_s	_soaring_estimator_shear{}; ///< soaring estimator shear
 	debug_value_s				_debug_value{};			// slip angle
 
 	// parameter struct
@@ -263,8 +245,6 @@ private:
 	void		vehicle_command_poll();
 	void		vehicle_control_mode_poll();
 	void		vehicle_status_poll();
-	void		soaring_controller_status_poll();
-	void		soaring_estimator_shear_poll();
 
 	//
 	void		status_publish();