diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index dbba029962..f0b05584cf 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -78,11 +78,6 @@ FixedwingPositionINDIControl::init() return false; } PX4_INFO("Starting FW_DYN_SOAR_CONTROLLER"); - //_read_trajectory_coeffs_csv("trajectory5.csv"); - //_read_trajectory_coeffs_csv("trajectory4.csv"); - //_read_trajectory_coeffs_csv("trajectory3.csv"); - //_read_trajectory_coeffs_csv("trajectory2.csv"); - //_read_trajectory_coeffs_csv("trajectory1.csv"); char filename[] = "trajectory0.csv"; _read_trajectory_coeffs_csv(filename); @@ -173,19 +168,20 @@ FixedwingPositionINDIControl::parameters_update() _loiter = _param_loiter.get(); - _shear_heading = _param_shear_heading.get()/180.f * M_PI_F + M_PI_2_F; - _shear_h_ref = _param_shear_height.get(); _select_trajectory(0.0f); - _thrust = _param_thrust.get(); - + _thrust_pos = _param_thrust.get(); + _thrust = _thrust_pos; _switch_saturation = _param_switch_saturation.get(); - _switch_filter = _param_switch_filter.get(); + _switch_origin_hardcoded = _param_switch_origin_hardcoded.get(); + // only update shear heading and height with params, if desired + if (_switch_origin_hardcoded) { + _shear_heading = _param_shear_heading.get()/180.f * M_PI_F + M_PI_2_F; + _shear_h_ref = _param_shear_height.get(); + } - // sanity check parameters - // TODO: include sanity check return PX4_OK; } @@ -289,13 +285,15 @@ FixedwingPositionINDIControl::manual_control_setpoint_poll() void FixedwingPositionINDIControl::rc_channels_poll() { - _rc_channels_sub.update(&_rc_channels); - // use flaps channel to select manual feedthrough - if (_rc_channels.channels[5]>=0.f){ - _switch_manual = 1; - } - else{ - _switch_manual = 0; + if (_rc_channels_sub.update(&_rc_channels)) { + // use flaps channel to select manual feedthrough + if (_rc_channels.channels[5]>=0.f){ + _switch_manual = 1; + } + else{ + _switch_manual = 0; + _thrust = _thrust_pos; + } } } @@ -409,6 +407,21 @@ FixedwingPositionINDIControl::_set_wind_estimate(Vector3f wind) return; } +void +FixedwingPositionINDIControl::soaring_estimator_shear_poll() +{ + if (_soaring_estimator_shear_sub.update(&_soaring_estimator_shear)){ + if (!_switch_origin_hardcoded) { + _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; + } + + // TODO: include some safety guards for large drifts, e.g. limit the maximum heading shift + } +} + float FixedwingPositionINDIControl::_getClosest(float val1, float val2, float target) { @@ -478,27 +491,22 @@ FixedwingPositionINDIControl::_select_trajectory(float initial_energy) in [0.20, 1.00] in [E_min, E_max] */ - - /* - float v = _findClosest(_v_max_arr, _gridsize, _shear_v_max); // wind velocity - float a = _findClosest(_alpha_arr, _gridsize, _shear_alpha); // shear strength - float e = _findClosest(_energy_arr, _gridsize, _shear_energy); // initial energy - PX4_INFO("V, A, E: \t%.2f\t%.2f\t%.2f", double(v), double(a), double(e)); + /* + float e = 20.f;//_findClosest(_energy_arr, _gridsize, _airspeed); // initial energy - char file[30] = "nominal"; + char file[30] = "trajec_robust"; char v_str[6]; char a_str[6]; char e_str[6]; strcat(file,"_"); - strcat(file,gcvt(v, 2, v_str)); + strcat(file,gcvt(_shear_v_max, 2, v_str)); strcat(file,"_"); - strcat(file,gcvt(a, 3, a_str)); + strcat(file,gcvt(_shear_alpha, 3, a_str)); strcat(file,"_"); strcat(file,gcvt(e, 2, e_str)); strcat(file,".csv"); + PX4_INFO("filename: \t%.30s", file); */ - //PX4_INFO("filename: \t%.30s", file); - // select loiter trajectory for loiter test char filename[16]; @@ -551,8 +559,8 @@ FixedwingPositionINDIControl::_read_trajectory_coeffs_csv(char *filename) // ======================================================================= bool error = false; - //char home_dir[200] = "/home/marvin/Documents/master_thesis_ADS/PX4/Git/ethzasl_fw_px4/src/modules/fw_dyn_soar_control/trajectories/"; - char home_dir[200] = PX4_ROOTFSDIR"/fs/microsd/trajectories/"; + char home_dir[200] = "/home/marvin/Documents/master_thesis_ADS/PX4/Git/ethzasl_fw_px4/src/modules/fw_dyn_soar_control/trajectories/"; + //char home_dir[200] = PX4_ROOTFSDIR"/fs/microsd/trajectories/"; //PX4_ERR(home_dir); strcat(home_dir,filename); FILE* fp = fopen(home_dir, "r"); @@ -733,6 +741,11 @@ FixedwingPositionINDIControl::Run() vehicle_angular_acceleration_poll(); soaring_controller_status_poll(); + // update the shear estimate, only if we are flying in manual feedthrough for safety reasons + if (_switch_manual) { + soaring_estimator_shear_poll(); + } + // update transform from trajectory frame to ENU frame (soaring frame) _compute_trajectory_transform(); diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp index 822a821c10..259a1cff38 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp @@ -69,6 +69,7 @@ #include #include #include +#include #include #include #include @@ -125,6 +126,7 @@ private: 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(actuator_controls_0)}; uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _rc_channels_sub{ORB_ID(rc_channels)}; @@ -164,6 +166,7 @@ private: 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 @@ -219,7 +222,9 @@ private: // force saturation (ParamInt) _param_switch_saturation, // command filtering - (ParamInt) _param_switch_filter + (ParamInt) _param_switch_filter, + // hardcoded trajectory center + (ParamInt) _param_switch_origin_hardcoded ) @@ -254,6 +259,7 @@ private: void vehicle_control_mode_poll(); void vehicle_status_poll(); void soaring_controller_status_poll(); + void soaring_estimator_shear_poll(); // void status_publish(); @@ -361,12 +367,15 @@ private: int _loiter; // thrust float _thrust; + float _thrust_pos; // controller mode bool _switch_manual = 1; // force limit bool _switch_saturation; // bool _switch_filter; + // + bool _switch_origin_hardcoded; bool _airspeed_valid{false}; ///< flag if a valid airspeed estimate exists hrt_abstime _airspeed_last_valid{0}; ///< last time airspeed was received. Used to detect timeouts. @@ -382,11 +391,7 @@ private: // vectors defining the gridding for trajectory selection: initial velocities, wind speed and shear strength const static size_t _gridsize = 11; - /* - float _energy_arr[_gridsize] = {14.f,16.f,18.f,20.f,22.f,24.f,26.f,28.f,30.f,32.f,34.f}; - float _v_max_arr[_gridsize] = {10.f,11.f,12.f,13.f,14.f,15.f,16.f,17.f,18.f,19.f,20.f}; - float _alpha_arr[_gridsize] = {0.1f,0.2f,0.3f,0.4f,0.5f,0.6f,0.7f,0.8f,0.9f,1.0f,1.1f}; - */ + // helper variables Dcmf _R_ned_to_enu; // rotation matrix from NED to ENU frame diff --git a/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c b/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c index 09524cba1c..e9bb204006 100644 --- a/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c +++ b/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c @@ -578,4 +578,19 @@ PARAM_DEFINE_INT32(DS_SWITCH_SAT, 1); * @increment 1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_INT32(DS_SWITCH_FILTER, 0); \ No newline at end of file +PARAM_DEFINE_INT32(DS_SWITCH_FILTER, 0); + +// ====================================================== +// ============= hardcoded trajectory center ============ +// ====================================================== + +/** + * integer in {0,1} defining if the trajectory origin is taken from hardcoded params or shear estimate, 1=params, 0=estimate + * @unit + * @min 0 + * @max 1 + * @decimal 1 + * @increment 1 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_INT32(DS_SWITCH_ORI_HC, 1); \ No newline at end of file diff --git a/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp b/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp index 7eeb6d7659..2ee3045fdb 100644 --- a/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp +++ b/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp @@ -341,12 +341,12 @@ FixedwingShearEstimator::Run() } // get the correct shear params for trajectory selection - float shear = sqrtf(powf(_X_posterior_horizontal(0)*_unit_v - _X_posterior_horizontal(2)*_unit_v, 2) + - powf(_X_posterior_horizontal(1)*_unit_v - _X_posterior_horizontal(3)*_unit_v, 2)); + float shear = sqrtf(powf(_X_posterior_horizontal(0)*_unit_v, 2) + + powf(_X_posterior_horizontal(1)*_unit_v, 2)); float v = _findClosest(_v_max_arr, 5, shear); // wind velocity float a = _findClosest(_alpha_arr, 9, _X_posterior_horizontal(5)*_unit_a); // shear strength - float heading = -atan2f(_X_posterior_horizontal(1)-_X_posterior_horizontal(3), - _X_posterior_horizontal(0)-_X_posterior_horizontal(2)); + float heading = atan2f(_X_posterior_horizontal(0), + _X_posterior_horizontal(1)); _soaring_estimator_shear.v_max = v; _soaring_estimator_shear.alpha = a; _soaring_estimator_shear.h_ref = _X_posterior_horizontal(4)*_unit_h; diff --git a/src/modules/fw_dyn_soar_estimator/fw_dyn_soar_estimator_params.c b/src/modules/fw_dyn_soar_estimator/fw_dyn_soar_estimator_params.c index 1554146a24..11dca4d71e 100644 --- a/src/modules/fw_dyn_soar_estimator/fw_dyn_soar_estimator_params.c +++ b/src/modules/fw_dyn_soar_estimator/fw_dyn_soar_estimator_params.c @@ -50,7 +50,7 @@ PARAM_DEFINE_FLOAT(DS_SIGMA_Q_H, 0.3f); * @increment 0.000001 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_SIGMA_Q_A, 0.02f); +PARAM_DEFINE_FLOAT(DS_SIGMA_Q_A, 0.01f); /** * Standard deviation of velicity measurement (wind)