diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index e2dc6eb24f..fdc23a889f 100644 --- a/msg/CMakeLists.txt +++ b/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 diff --git a/msg/soaring_controller_heartbeat.msg b/msg/soaring_controller_heartbeat.msg deleted file mode 100644 index 1d699c277a..0000000000 --- a/msg/soaring_controller_heartbeat.msg +++ /dev/null @@ -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 diff --git a/msg/soaring_controller_position.msg b/msg/soaring_controller_position.msg deleted file mode 100644 index f77f6799b0..0000000000 --- a/msg/soaring_controller_position.msg +++ /dev/null @@ -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 \ No newline at end of file diff --git a/msg/soaring_controller_position_setpoint.msg b/msg/soaring_controller_position_setpoint.msg deleted file mode 100644 index d756f9d8ec..0000000000 --- a/msg/soaring_controller_position_setpoint.msg +++ /dev/null @@ -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 - - diff --git a/msg/soaring_controller_status.msg b/msg/soaring_controller_status.msg deleted file mode 100644 index a9eee91e72..0000000000 --- a/msg/soaring_controller_status.msg +++ /dev/null @@ -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 \ No newline at end of file diff --git a/msg/soaring_controller_wind.msg b/msg/soaring_controller_wind.msg deleted file mode 100644 index 399ca9392b..0000000000 --- a/msg/soaring_controller_wind.msg +++ /dev/null @@ -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 diff --git a/msg/soaring_estimator_shear.msg b/msg/soaring_estimator_shear.msg deleted file mode 100644 index 53f94797a7..0000000000 --- a/msg/soaring_estimator_shear.msg +++ /dev/null @@ -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 diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index 98a806f756..90d895d57b 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/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 // ==================== diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp index b17290366e..afb1ecb52c 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp @@ -63,12 +63,6 @@ #include #include #include -#include -#include -#include -#include -#include -#include #include #include #include @@ -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 _angular_vel_sp_pub{ORB_ID(vehicle_rates_setpoint)}; uORB::Publication _angular_accel_sp_pub{ORB_ID(vehicle_angular_acceleration_setpoint)}; uORB::PublicationMulti _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)}; - uORB::Publication _soaring_controller_heartbeat_pub{ORB_ID(soaring_controller_status)}; - uORB::Publication _soaring_controller_position_setpoint_pub{ORB_ID(soaring_controller_position_setpoint)}; - uORB::Publication _soaring_controller_position_pub{ORB_ID(soaring_controller_position)}; - uORB::Publication _soaring_controller_wind_pub{ORB_ID(soaring_controller_wind)}; uORB::Publication _offboard_control_mode_pub{ORB_ID(offboard_control_mode)}; uORB::Publication _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();