From f4a2e0db406c307bd7d77123713884cbc1487be3 Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Tue, 8 Jul 2025 15:39:54 +0900 Subject: [PATCH] Rebase fixes Initialize local origin F F Disable manual switch --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 2 +- .../FixedwingPositionINDIControl.cpp | 204 +++++++----------- .../FixedwingPositionINDIControl.hpp | 62 +++--- .../fw_dyn_soar_control_params.c | 190 ++++++++-------- 4 files changed, 202 insertions(+), 256 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index 2124170c20..a9fd37eb4b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -13,7 +13,7 @@ control_allocator start # # Start attitude controller. # -fw_indi_pos_control start +fw_dyn_soar_control start fw_rate_control start fw_att_control start fw_mode_manager start diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index 18908f5632..5fa1da6f6b 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -53,7 +53,6 @@ using matrix::wrap_pi; FixedwingPositionINDIControl::FixedwingPositionINDIControl() : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), - _actuators_0_pub(ORB_ID(actuator_controls_0)), _attitude_sp_pub(ORB_ID(vehicle_attitude_setpoint)), _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) { @@ -172,13 +171,14 @@ FixedwingPositionINDIControl::parameters_update() // check if local NED reference frame origin has changed: // || (_local_pos.vxy_reset_counter != _pos_reset_counter // initialize projection - map_projection_init_timestamped(&_global_local_proj_ref, _local_pos.ref_lat, _local_pos.ref_lon, - _local_pos.ref_timestamp); - // project the origin of the soaring ENU frame to the current NED frame - map_projection_project(&_global_local_proj_ref, _origin_lat, _origin_lon, &_origin_N, &_origin_E); + _global_local_proj_ref.initReference(_local_pos.ref_lat, _local_pos.ref_lon, + _local_pos.ref_timestamp); + matrix::Vector2f origin_NE = _global_local_proj_ref.project(_origin_lat, _origin_lon); + _origin_N = origin_NE(0); + _origin_E = origin_NE(1); _origin_D = _local_pos.ref_alt - _origin_alt; PX4_INFO("local reference frame updated"); - + _thrust_pos = _param_thrust.get(); _thrust = _thrust_pos; _switch_saturation = _param_switch_saturation.get(); @@ -230,58 +230,6 @@ FixedwingPositionINDIControl::airspeed_poll() _airspeed_valid = airspeed_valid; } -void -FixedwingPositionINDIControl::airflow_aoa_poll() -{ - bool aoa_valid = _aoa_valid; - airflow_aoa_s aoa_validated; - - if (_airflow_aoa_sub.update(&aoa_validated)) { - - if (PX4_ISFINITE(aoa_validated.aoa_rad) - && (aoa_validated.valid)) { - - aoa_valid = true; - - _aoa_last_valid = aoa_validated.timestamp; - _aoa = aoa_validated.aoa_rad; - } - - } else { - // no aoa updates for one second - if (aoa_valid && (hrt_elapsed_time(&_aoa_last_valid) > 1_s)) { - aoa_valid = false; - } - } - _aoa_valid = aoa_valid; -} - -void -FixedwingPositionINDIControl::airflow_slip_poll() -{ - bool slip_valid = _slip_valid; - airflow_slip_s slip_validated; - - if (_airflow_slip_sub.update(&slip_validated)) { - - if (PX4_ISFINITE(slip_validated.slip_rad) - && (slip_validated.valid)) { - - slip_valid = true; - - _slip_last_valid = slip_validated.timestamp; - _slip = slip_validated.slip_rad; - } - - } else { - // no aoa updates for one second - if (slip_valid && (hrt_elapsed_time(&_slip_last_valid) > 1_s)) { - slip_valid = false; - } - } - _slip_valid = slip_valid; -} - void FixedwingPositionINDIControl::vehicle_status_poll() { @@ -295,7 +243,7 @@ FixedwingPositionINDIControl::manual_control_setpoint_poll() { if(_switch_manual){ _manual_control_setpoint_sub.update(&_manual_control_setpoint); - _thrust = _manual_control_setpoint.z; + _thrust = _manual_control_setpoint.throttle; } else { _thrust = _thrust_pos; @@ -339,22 +287,12 @@ FixedwingPositionINDIControl::vehicle_angular_velocity_poll() // no need to check if it was updated as the main loop is fired based on an update... // _omega = Vector3f(_angular_vel.xyz); + _alpha = Vector3f(_angular_vel.xyz_derivative); if(hrt_absolute_time()-_angular_vel.timestamp > 20_ms && _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD){ PX4_ERR("angular velocity sample is too old"); } } -void -FixedwingPositionINDIControl::vehicle_angular_acceleration_poll() -{ - if (_vehicle_angular_acceleration_sub.update(&_angular_accel)) { - _alpha = Vector3f(_angular_accel.xyz); - } - if(hrt_absolute_time()-_angular_accel.timestamp > 20_ms && _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD){ - PX4_ERR("angular acceleration sample is too old"); - } -} - void FixedwingPositionINDIControl::vehicle_local_position_poll() { @@ -417,7 +355,7 @@ FixedwingPositionINDIControl::soaring_estimator_shear_poll() // the initial speed of the target trajectory can safely be updated during soaring :) _shear_aspd = _soaring_estimator_shear.aspd; } - + } } @@ -443,7 +381,7 @@ FixedwingPositionINDIControl::_compute_wind_estimate() // as L = alpha*Fx - Fz float Fx = cosf(_aoa_offset)*body_force(0) - sinf(_aoa_offset)*body_force(2); float Fz = -cosf(_aoa_offset)*body_force(2) - sinf(_aoa_offset)*body_force(0); - float AoA_approx = (((2.f*Fz)/(_rho*_area*(fmaxf(_cal_airspeed*_cal_airspeed,_stall_speed*_stall_speed))+0.001f) - _C_L0)/_C_L1) / + float AoA_approx = (((2.f*Fz)/(_rho*_area*(fmaxf(_cal_airspeed*_cal_airspeed,_stall_speed*_stall_speed))+0.001f) - _C_L0)/_C_L1) / (1 - ((2.f*Fx)/(_rho*_area*(fmaxf(_cal_airspeed*_cal_airspeed,_stall_speed*_stall_speed))+0.001f)/_C_L1)); AoA_approx = constrain(AoA_approx,-0.2f,0.3f); float speed = fmaxf(_cal_airspeed, _stall_speed); @@ -481,7 +419,7 @@ FixedwingPositionINDIControl::_compute_wind_estimate_EKF() void FixedwingPositionINDIControl::_set_wind_estimate(Vector3f wind) -{ +{ // apply some filtering wind(0) = _lp_filter_wind[0].apply(wind(0)); wind(1) = _lp_filter_wind[1].apply(wind(1)); @@ -492,7 +430,7 @@ FixedwingPositionINDIControl::_set_wind_estimate(Vector3f wind) void FixedwingPositionINDIControl::_set_wind_estimate_EKF(Vector3f wind) -{ +{ // apply some filtering wind(0) = _lp_filter_wind_EKF[0].apply(wind(0)); wind(1) = _lp_filter_wind_EKF[1].apply(wind(1)); @@ -502,7 +440,7 @@ FixedwingPositionINDIControl::_set_wind_estimate_EKF(Vector3f wind) } -void +void FixedwingPositionINDIControl::_reverse(char* str, int len) { // copied from: https://www.geeksforgeeks.org/convert-floating-point-number-string/ @@ -516,7 +454,7 @@ FixedwingPositionINDIControl::_reverse(char* str, int len) } } -int +int FixedwingPositionINDIControl::_int_to_str(int x, char str[], int d) { // copied from: https://www.geeksforgeeks.org/convert-floating-point-number-string/ @@ -525,12 +463,12 @@ FixedwingPositionINDIControl::_int_to_str(int x, char str[], int d) str[i++] = (x % 10) + '0'; x = x / 10; } - + // If number of digits required is more, then // add 0s at the beginning while (i < d) str[i++] = '0'; - + _reverse(str, i); str[i] = '\0'; return i; @@ -542,22 +480,22 @@ FixedwingPositionINDIControl::_float_to_str(float n, char* res, int afterpoint) // copied from: https://www.geeksforgeeks.org/convert-floating-point-number-string/ // Extract integer part int ipart = (int)n; - + // Extract floating part float fpart = n - (float)ipart; - + // convert integer part to string int i = _int_to_str(ipart, res, 0); - + // check for display option after point if (afterpoint != 0) { res[i] = '.'; // add dot - + // Get the value of fraction part upto given no. // of points after dot. The third parameter // is needed to handle cases like 233.007 fpart = fpart * (float)pow(10, afterpoint); - + _int_to_str((int)fpart, res + i + 1, afterpoint); } } @@ -601,22 +539,22 @@ FixedwingPositionINDIControl::_select_loiter_trajectory() /* Also, we need to place the current trajectory, centered around zero height and assuming wind from the west, into the soaring frame. - Since the necessary computations for position, velocity and acceleration require the basis coefficients, which are not easy to transform, + Since the necessary computations for position, velocity and acceleration require the basis coefficients, which are not easy to transform, we choose to transform our position in soaring frame into the "trajectory frame", compute position vector and it's derivatives from the basis coeffs in the trajectory frame, and then transform these back to soaring frame for control purposes. Therefore we define a new transform between the soaring frame and the trajectory frame. */ - + _read_trajectory_coeffs_csv(filename); } void FixedwingPositionINDIControl::_select_soaring_trajectory() { - /* + /* We read a trajectory based on initial energy available at the beginning of the trajectory. So the trajectory is selected based on two criteria, first the correct wind shear params (alpha and V_max) and the initial energy (potential + kinetic). - + The filename structure of the trajectories is the following: trajec_____ with @@ -654,8 +592,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/jaeyoung/src/PX4-Autopilot/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"); @@ -677,10 +615,10 @@ FixedwingPositionINDIControl::_read_trajectory_coeffs_csv(char *filename) while (fgets(buffer, buffersize, fp)) { column = 0; - + // Splitting the data char* value = strtok(buffer, ","); - + // loop over columns while (value) { if (*value=='\0'||*value==' ') { @@ -704,7 +642,7 @@ FixedwingPositionINDIControl::_read_trajectory_coeffs_csv(char *filename) //PX4_INFO("row: %d, col: %d, read value: %.3f", row, column, (double)atof(value)); value = strtok(NULL, ","); column++; - + } row++; } @@ -786,7 +724,7 @@ FixedwingPositionINDIControl::Run() // only run controller if pos, vel, acc changed if (_vehicle_angular_velocity_sub.update(&_angular_vel)) - { + { // only update parameters if they changed bool params_updated = _parameter_update_sub.updated(); @@ -806,15 +744,27 @@ FixedwingPositionINDIControl::Run() // check if local NED reference frame origin has changed: // || (_local_pos.vxy_reset_counter != _pos_reset_counter - if (!map_projection_initialized(&_global_local_proj_ref) - || (_global_local_proj_ref.timestamp != _local_pos.ref_timestamp) + if (!_global_local_proj_ref.isInitialized() + || (_global_local_proj_ref.getProjectionReferenceTimestamp() != _local_pos.ref_timestamp) || (_local_pos.xy_reset_counter != _pos_reset_counter) || (_local_pos.z_reset_counter != _alt_reset_counter)) { + + double reference_latitude = 0.; + double reference_longitude = 0.; + + if (_local_pos.xy_global && PX4_ISFINITE(_local_pos.ref_lat) && PX4_ISFINITE(_local_pos.ref_lon)) { + reference_latitude = _local_pos.ref_lat; + reference_longitude = _local_pos.ref_lon; + } + // initialize projection - map_projection_init_timestamped(&_global_local_proj_ref, _local_pos.ref_lat, _local_pos.ref_lon, - _local_pos.ref_timestamp); - // project the origin of the soaring ENU frame to the current NED frame - map_projection_project(&_global_local_proj_ref, _origin_lat, _origin_lon, &_origin_N, &_origin_E); + _global_local_proj_ref.initReference(reference_latitude, reference_longitude, + _local_pos.ref_timestamp); + + matrix::Vector2f origin_NE = _global_local_proj_ref.project(_origin_lat, _origin_lon); + _origin_N = origin_NE(0); + _origin_E = origin_NE(1); + _origin_D = _local_pos.ref_alt - _origin_alt; PX4_INFO("local reference frame updated"); } @@ -825,15 +775,12 @@ FixedwingPositionINDIControl::Run() // run polls vehicle_status_poll(); airspeed_poll(); - airflow_aoa_poll(); - airflow_slip_poll(); rc_channels_poll(); manual_control_setpoint_poll(); vehicle_local_position_poll(); vehicle_attitude_poll(); vehicle_acceleration_poll(); vehicle_angular_velocity_poll(); - vehicle_angular_acceleration_poll(); soaring_controller_status_poll(); // update the shear estimate, only target airspeed is updated in soaring mode @@ -872,7 +819,7 @@ FixedwingPositionINDIControl::Run() // ============================ // compute reference kinematics // ============================ - // downscale velocity to match current one, + // downscale velocity to match current one, // terminal time is determined such that current velocity is met Vector3f v_ref_ = _get_velocity_ref(t_ref, 1.0f); float T = sqrtf((v_ref_*v_ref_)/(_vel*_vel+0.001f)); @@ -898,13 +845,13 @@ FixedwingPositionINDIControl::Run() // publish offboard control commands // ================================= offboard_control_mode_s ocm{}; - ocm.actuator = true; + ocm.thrust_and_torque = true; ocm.timestamp = hrt_absolute_time(); _offboard_control_mode_pub.publish(ocm); // Publish actuator controls only once in OFFBOARD if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) { - + // ======================================== // publish controller position in ENU frame // ======================================== @@ -933,7 +880,7 @@ FixedwingPositionINDIControl::Run() // ===================== // publish control input // ===================== - //_angular_accel_sp = {}; + //_angular_accel_sp = {}; _angular_accel_sp.timestamp = hrt_absolute_time(); //_angular_accel_sp.timestamp_sample = hrt_absolute_time(); _angular_accel_sp.xyz[0] = ctrl(0); @@ -962,18 +909,17 @@ FixedwingPositionINDIControl::Run() _angular_vel_sp.pitch = omega_ref(1); _angular_vel_sp.yaw = omega_ref(2); _angular_vel_sp_pub.publish(_angular_vel_sp); - + // ========================= // publish acutator controls // ========================= - //_actuators = {}; + _actuators = {}; _actuators.timestamp = hrt_absolute_time(); _actuators.timestamp_sample = hrt_absolute_time(); - _actuators.control[actuator_controls_s::INDEX_ROLL] = ctrl2(0); - _actuators.control[actuator_controls_s::INDEX_PITCH] = ctrl2(1); - _actuators.control[actuator_controls_s::INDEX_YAW] = ctrl2(2); - _actuators.control[actuator_controls_s::INDEX_THROTTLE] = _thrust; - _actuators_0_pub.publish(_actuators); + ctrl2.copyTo(_actuators.xyz); + _torque_sp_pub.publish(_actuators); + _thrust_sp.xyz[0] = _thrust; + _thrust_sp_pub.publish(_thrust_sp); //print_message(_actuators); // ===================== @@ -1052,9 +998,9 @@ FixedwingPositionINDIControl::Run() _debug_value.value = _slip; _debug_value_pub.publish(_debug_value); - perf_end(_loop_perf); + perf_end(_loop_perf); } - + } Vector @@ -1096,7 +1042,7 @@ FixedwingPositionINDIControl::_get_d2_dt2_basis_funs(float t) float fun2 = exp(-powf((t-float(i)/_num_basis_funs),2)/sigma); vec(i) = fun2 * (fun1 * (4*powf((float(i)/_num_basis_funs-t),2) - \ sigma*(powf(M_PI_F,2)*sigma + 2)) + 4*M_PI_F*sigma*(float(i)/_num_basis_funs-t)*cosf(M_PI_F*t))/(powf(sigma,2)); - + } return vec; } @@ -1182,8 +1128,8 @@ FixedwingPositionINDIControl::_get_attitude_ref(float t, float T) Rotation(2,2) *= -1.f; // plausibility check /* - float determinant = Rotation(0,0)*(Rotation(1,1)*Rotation(2,2)-Rotation(2,1)*Rotation(1,2)) - - Rotation(1,0)*(Rotation(0,1)*Rotation(2,2)-Rotation(2,1)*Rotation(0,2)) + + float determinant = Rotation(0,0)*(Rotation(1,1)*Rotation(2,2)-Rotation(2,1)*Rotation(1,2)) - + Rotation(1,0)*(Rotation(0,1)*Rotation(2,2)-Rotation(2,1)*Rotation(0,2)) + Rotation(2,0)*(Rotation(0,1)*Rotation(1,2)-Rotation(1,1)*Rotation(0,2)); PX4_INFO("determinant: %.2f", (double)determinant); PX4_INFO("length: %.2f", (double)(wing_normalized*wing_normalized)); @@ -1353,7 +1299,7 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v // compute expected aerodynamic force // ================================== Vector3f f_current; - Vector3f vel_body = R_bi*(_vel - _wind_estimate); + Vector3f vel_body = R_bi*(_vel - _wind_estimate); float AoA = atan2f(vel_body(2), vel_body(0)) + _aoa_offset; float C_l = _C_L0 + _C_L1*AoA; float C_d = _C_D0 + _C_D1*AoA + _C_D2*powf(AoA,2); @@ -1447,8 +1393,8 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v // ==================================== if (_switch_manual){ // get an attitude setpoint from the current manual inputs - float roll_ref = 1.f * _manual_control_setpoint.y * 1.0f; - float pitch_ref = -1.f* _manual_control_setpoint.x * M_PI_4_F; + float roll_ref = 1.f * _manual_control_setpoint.roll * 1.0f; + float pitch_ref = -1.f* _manual_control_setpoint.pitch * M_PI_4_F; Eulerf E_current(Quatf(_attitude.q)); float yaw_ref = E_current.psi(); Dcmf R_ned_frd_ref(Eulerf(roll_ref, pitch_ref, yaw_ref)); @@ -1476,7 +1422,7 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v // compute rot acc command rot_acc_command = _K_q*w_err + _K_w*(Vector3f{0.f,0.f,0.f}-_omega); - + } // ============================================================== @@ -1501,17 +1447,17 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v // apply some smoothing since we don't want HF components in our rudder output omega_turn_ref(2) = _lp_filter_rud.apply(omega_turn_ref(2)); - + // transform rate vector to body frame float scaler = (_stall_speed*_stall_speed)/fmaxf(sqrtf(vel_body*vel_body)*vel_body(0), _stall_speed*_stall_speed); // not really an accel command, rather a FF-P command rot_acc_command(2) = _K_q(2,2)*omega_turn_ref(2)*scaler + _K_w(2,2)*(omega_turn_ref(2) - omega_filtered(2))* scaler*scaler; - + return rot_acc_command; } -Vector3f +Vector3f FixedwingPositionINDIControl::_compute_INDI_stage_2(Vector3f ctrl) { // compute velocity in body frame @@ -1521,15 +1467,15 @@ FixedwingPositionINDIControl::_compute_INDI_stage_2(Vector3f ctrl) // compute moments Vector3f moment; - moment(0) = _k_ail*q*_actuators.control[actuator_controls_s::INDEX_ROLL] - _k_d_roll*q*_omega(0); - moment(1) = _k_ele*q*_actuators.control[actuator_controls_s::INDEX_PITCH] - _k_d_pitch*q*_omega(1); + moment(0) = _k_ail*q*_actuators.xyz[0] - _k_d_roll*q*_omega(0); + moment(1) = _k_ele*q*_actuators.xyz[1] - _k_d_pitch*q*_omega(1); moment(2) = 0.f; // introduce artificial time delay that is also present in acceleration Vector3f moment_filtered; moment_filtered(0) = _lp_filter_delay[0].apply(moment(0)); moment_filtered(1) = _lp_filter_delay[1].apply(moment(1)); - moment_filtered(2) = _lp_filter_delay[2].apply(moment(2)); + moment_filtered(2) = _lp_filter_delay[2].apply(moment(2)); // No filter for alpha, since it is already filtered... Vector3f alpha_filtered = _alpha; @@ -1550,7 +1496,7 @@ FixedwingPositionINDIControl::_compute_INDI_stage_2(Vector3f ctrl) Vector3f FixedwingPositionINDIControl::_compute_actuator_deflections(Vector3f ctrl) -{ +{ // compute the normalized actuator deflection, including airspeed scaling Vector3f deflection = ctrl; diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp index 4beca7a009..45de21e031 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp @@ -18,13 +18,13 @@ #include #include -#include "fw_att_control/ecl_pitch_controller.h" -#include "fw_att_control/ecl_roll_controller.h" -#include "fw_att_control/ecl_wheel_controller.h" -#include "fw_att_control/ecl_yaw_controller.h" -#include +#include "fw_att_control/fw_pitch_controller.h" +#include "fw_att_control/fw_roll_controller.h" +#include "fw_att_control/fw_wheel_controller.h" +#include "fw_att_control/fw_yaw_controller.h" +#include #include -#include +// #include #include #include #include @@ -41,8 +41,8 @@ #include #include #include -#include -#include +// #include +// #include #include #include #include @@ -51,7 +51,6 @@ #include #include #include -#include #include #include #include @@ -62,8 +61,8 @@ #include #include #include -#include -#include +#include +#include #include #include #include @@ -118,21 +117,22 @@ private: // Subscriptions uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; // vehicle status uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; // airspeed - uORB::Subscription _airflow_aoa_sub{ORB_ID(airflow_aoa)}; // angle of attack - uORB::Subscription _airflow_slip_sub{ORB_ID(airflow_slip)}; // angle of sideslip +// uORB::Subscription _airflow_aoa_sub{ORB_ID(airflow_aoa)}; // angle of attack +// uORB::Subscription _airflow_slip_sub{ORB_ID(airflow_slip)}; // angle of sideslip uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; // linear acceleration uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; // local NED position 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 _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 _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)}; // Publishers - uORB::Publication _actuators_0_pub; + uORB::Publication _torque_sp_pub{ORB_ID(vehicle_torque_setpoint)}; + uORB::Publication _thrust_sp_pub{ORB_ID(vehicle_thrust_setpoint)}; uORB::Publication _attitude_sp_pub; uORB::Publication _angular_vel_sp_pub{ORB_ID(vehicle_rates_setpoint)}; uORB::Publication _angular_accel_sp_pub{ORB_ID(vehicle_angular_acceleration_setpoint)}; @@ -146,7 +146,8 @@ private: // Message structs vehicle_angular_acceleration_setpoint_s _angular_accel_sp {}; - actuator_controls_s _actuators {}; // actuator commands + vehicle_torque_setpoint_s _actuators {}; // actuator commands + vehicle_thrust_setpoint_s _thrust_sp {}; manual_control_setpoint_s _manual_control_setpoint {}; ///< r/c channel data rc_channels_s _rc_channels {}; ///< rc channels vehicle_local_position_s _local_pos {}; ///< vehicle local position @@ -155,9 +156,9 @@ private: vehicle_attitude_setpoint_s _attitude_sp {}; ///< vehicle attitude setpoint vehicle_angular_velocity_s _angular_vel {}; ///< vehicle angular velocity vehicle_rates_setpoint_s _angular_vel_sp {}; ///< vehicle angular velocity setpoint - vehicle_angular_acceleration_s _angular_accel {}; ///< vehicle angular acceleration + matrix::Vector3f _angular_accel {}; ///< vehicle angular acceleration home_position_s _home_pos {}; ///< home position - map_projection_reference_s _global_local_proj_ref{}; + MapProjection _global_local_proj_ref{}; 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 @@ -247,14 +248,13 @@ private: // Update subscriptions void wind_poll(); void airspeed_poll(); - void airflow_aoa_poll(); - void airflow_slip_poll(); + // void airflow_aoa_poll(); + // void airflow_slip_poll(); void vehicle_local_position_poll(); void vehicle_acceleration_poll(); void vehicle_attitude_poll(); void vehicle_angular_velocity_poll(); - void vehicle_angular_acceleration_poll(); void actuator_controls_poll(); void control_update(); @@ -330,21 +330,21 @@ private: const float _sample_frequency = 200.f; // Low-Pass filters stage 1 const float _cutoff_frequency_1 = 20.f; - math::LowPassFilter2p _lp_filter_accel[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}}; // linear acceleration - math::LowPassFilter2p _lp_filter_force[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}}; // force command - math::LowPassFilter2p _lp_filter_omega[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}}; // body rates + math::LowPassFilter2p _lp_filter_accel[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}}; // linear acceleration + math::LowPassFilter2p _lp_filter_force[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}}; // force command + math::LowPassFilter2p _lp_filter_omega[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}}; // body rates // smoothing filter to reject HF noise in control output const float _cutoff_frequency_smoothing = 20.f; // we want to attenuate noise at 30Hz with -10dB -> need cutoff frequency 5 times lower (6Hz) - math::LowPassFilter2p _lp_filter_ctrl0[3] {{_sample_frequency, _cutoff_frequency_smoothing}, {_sample_frequency, _cutoff_frequency_smoothing}, {_sample_frequency, _cutoff_frequency_smoothing}}; // force command stage 1 - math::LowPassFilter2p _lp_filter_rud {_sample_frequency, 10}; // rudder command + math::LowPassFilter2p _lp_filter_ctrl0[3] {{_sample_frequency, _cutoff_frequency_smoothing}, {_sample_frequency, _cutoff_frequency_smoothing}, {_sample_frequency, _cutoff_frequency_smoothing}}; // force command stage 1 + math::LowPassFilter2p _lp_filter_rud {_sample_frequency, 10}; // rudder command // Low-Pass filters stage 2 const float _cutoff_frequency_2 = 20.f; // MUST MATCH PARAM "IMU_DGYRO_CUTOFF" - math::LowPassFilter2p _lp_filter_delay[3] {{_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}}; // filter to match acceleration processing delay - math::LowPassFilter2p _lp_filter_omega_2[3] {{_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}}; // body rates + math::LowPassFilter2p _lp_filter_delay[3] {{_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}}; // filter to match acceleration processing delay + math::LowPassFilter2p _lp_filter_omega_2[3] {{_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}}; // body rates // Low-Pass filter for wind estimate const float _cutoff_frequency_wind = 1.f; - math::LowPassFilter2p _lp_filter_wind[3] {{_sample_frequency, _cutoff_frequency_wind}, {_sample_frequency, _cutoff_frequency_wind}, {_sample_frequency, _cutoff_frequency_wind}}; // wind_estimate inside controller - math::LowPassFilter2p _lp_filter_wind_EKF[3] {{_sample_frequency, _cutoff_frequency_wind}, {_sample_frequency, _cutoff_frequency_wind}, {_sample_frequency, _cutoff_frequency_wind}}; // wind_estimate for EKF + math::LowPassFilter2p _lp_filter_wind[3] {{_sample_frequency, _cutoff_frequency_wind}, {_sample_frequency, _cutoff_frequency_wind}, {_sample_frequency, _cutoff_frequency_wind}}; // wind_estimate inside controller + math::LowPassFilter2p _lp_filter_wind_EKF[3] {{_sample_frequency, _cutoff_frequency_wind}, {_sample_frequency, _cutoff_frequency_wind}, {_sample_frequency, _cutoff_frequency_wind}}; // wind_estimate for EKF uint _counter = 0; hrt_abstime _last_time{0}; 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 4168d7ce67..52ab57b1a1 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 @@ -12,9 +12,9 @@ /** * inertia around body x-axis - * + * * This is the inertia of the aircraft, used for the INDI - * + * * @unit kg * @min 0.01 * @max 10 @@ -26,9 +26,9 @@ PARAM_DEFINE_FLOAT(DS_INERTIA_ROLL, 0.197563f); /** * inertia around body y-axis - * + * * This is the inertia of the aircraft, used for the INDI - * + * * @unit kg * @min 0.01 * @max 10 @@ -40,9 +40,9 @@ PARAM_DEFINE_FLOAT(DS_INERTIA_PITCH, 0.1458929f); /** * inertia around body z-axis - * + * * This is the inertia of the aircraft, used for the INDI - * + * * @unit kg * @min 0.01 * @max 10 @@ -54,9 +54,9 @@ PARAM_DEFINE_FLOAT(DS_INERTIA_YAW, 0.1477f); /** * inertia tensor term in body xz-axis (roll-yaw coupling) - * + * * This is the inertia of the aircraft, used for the INDI - * + * * @unit kg * @min -0.5 * @max 0 @@ -68,9 +68,9 @@ PARAM_DEFINE_FLOAT(DS_INERTIA_RP, -0.0f); /** * total takeoff mass - * + * * This is the mass of the aircraft, used for the INDI - * + * * @unit kg * @min 1.0 * @max 2.0 @@ -82,7 +82,7 @@ PARAM_DEFINE_FLOAT(DS_MASS, 1.4f); /** * total wing area used for lift and drag computation - * + * * @unit m^2 * @min 0.1 * @max 1.0 @@ -95,8 +95,8 @@ PARAM_DEFINE_FLOAT(DS_WING_AREA, 0.4f); /** * air density used for lift and drag computation - * - * @unit + * + * @unit * @min 0.5 * @max 1.225 * @decimal 3 @@ -107,11 +107,11 @@ PARAM_DEFINE_FLOAT(DS_RHO, 1.223f); /** * estimated lift coefficients used for lift and drag computation - * + * * Used as L = C_l0 + C_l1*alpha, * where alpha is the angle of attack. - * - * @unit + * + * @unit * @min -100 * @max 100 * @decimal 3 @@ -122,11 +122,11 @@ PARAM_DEFINE_FLOAT(DS_C_L0, 0.356f); /** * estimated lift coefficients used for lift and drag computation - * + * * Used as L = C_l0 + C_l1*alpha, * where alpha is the angle of attack. - * - * @unit + * + * @unit * @min -100 * @max 100 * @decimal 3 @@ -138,11 +138,11 @@ PARAM_DEFINE_FLOAT(DS_C_L1, 2.354f); /** * estimated drag coefficients used for lift and drag computation - * + * * Used as D = C_d0 + C_d1*alpha + C_d2*alpha**2, * where alpha is the angle of attack. - * - * @unit + * + * @unit * @min -100 * @max 100 * @decimal 4 @@ -153,11 +153,11 @@ PARAM_DEFINE_FLOAT(DS_C_D0, 0.0288f); /** * estimated drag coefficients used for lift and drag computation - * + * * Used as D = C_d0 + C_d1*alpha + C_d2*alpha**2, * where alpha is the angle of attack. - * - * @unit + * + * @unit * @min -100 * @max 100 * @decimal 4 @@ -168,11 +168,11 @@ PARAM_DEFINE_FLOAT(DS_C_D1, 0.3783f); /** * estimated drag coefficients used for lift and drag computation - * + * * Used as D = C_d0 + C_d1*alpha + C_d2*alpha**2, * where alpha is the angle of attack. - * - * @unit + * + * @unit * @min -100 * @max 100 * @decimal 4 @@ -183,11 +183,11 @@ PARAM_DEFINE_FLOAT(DS_C_D2, 1.984f); /** * estimated sideslip sensitivity coefficients used for wind estimation - * + * * Used as F_y = 0.5 * DS_RHO * ASPD^2 * DS_C_B1, * where alpha is the angle of attack. - * - * @unit + * + * @unit * @min -100 * @max -0.01 * @decimal 4 @@ -198,9 +198,9 @@ PARAM_DEFINE_FLOAT(DS_C_B1, -3.32f); /** * offset angle between body frame (Pixhawk) and the wing chord - * + * * Used to compute the AoA - * + * * @unit rad * @min 0 * @max 0.1 @@ -212,7 +212,7 @@ PARAM_DEFINE_FLOAT(DS_AOA_OFFSET, 0.07f); /** * stall speed of the aircraft - * + * * @unit m/s * @min 5 * @max 10 @@ -228,8 +228,8 @@ PARAM_DEFINE_FLOAT(DS_STALL_SPEED, 9.0f); // ======================================================== /** * control gain of position PD-controller (body x-direction) - * - * @unit + * + * @unit * @min 0 * @max 100 * @decimal 1 @@ -240,8 +240,8 @@ PARAM_DEFINE_FLOAT(DS_LIN_K_X, 1.0f); /** * control gain of position PD-controller (body y-direction) - * - * @unit + * + * @unit * @min 0 * @max 100 * @decimal 1 @@ -252,8 +252,8 @@ PARAM_DEFINE_FLOAT(DS_LIN_K_Y, 1.0f); /** * control gain of position PD-controller (body z-direction) - * - * @unit + * + * @unit * @min 0 * @max 100 * @decimal 1 @@ -264,8 +264,8 @@ PARAM_DEFINE_FLOAT(DS_LIN_K_Z, 1.0f); /** * normalized damping coefficient of position PD-controller (body x-direction) - * - * @unit + * + * @unit * @min 0 * @max 2 * @decimal 2 @@ -276,8 +276,8 @@ PARAM_DEFINE_FLOAT(DS_LIN_C_X, 1.0f); /** * normalized damping coefficient of position PD-controller (body y-direction) - * - * @unit + * + * @unit * @min 0 * @max 2 * @decimal 2 @@ -288,8 +288,8 @@ PARAM_DEFINE_FLOAT(DS_LIN_C_Y, 1.0f); /** * normalized damping coefficient of position PD-controller (body z-direction) - * -* @unit + * +* @unit * @min 0 * @max 2 * @decimal 2 @@ -300,8 +300,8 @@ PARAM_DEFINE_FLOAT(DS_LIN_C_Z, 1.0f); /** * acceleration feedback gain of position PD-controller (body x-direction) - * - * @unit + * + * @unit * @min 0 * @max 10 * @decimal 1 @@ -312,8 +312,8 @@ PARAM_DEFINE_FLOAT(DS_LIN_FF_X, 0.5f); /** * acceleration feedback gain of position PD-controller (body y-direction) - * - * @unit + * + * @unit * @min 0 * @max 10 * @decimal 1 @@ -324,8 +324,8 @@ PARAM_DEFINE_FLOAT(DS_LIN_FF_Y, 0.5f); /** * acceleration feedback gain of position PD-controller (body z-direction) - * - * @unit + * + * @unit * @min 0 * @max 10 * @decimal 1 @@ -336,8 +336,8 @@ PARAM_DEFINE_FLOAT(DS_LIN_FF_Z, 0.5f); /** * control gain of attitude PD-controller (body roll-direction) - * - * @unit + * + * @unit * @min 0 * @max 100 * @decimal 1 @@ -348,8 +348,8 @@ PARAM_DEFINE_FLOAT(DS_ROT_K_ROLL, 30.0f); /** * control gain of attitude PD-controller (body pitch-direction) - * - * @unit + * + * @unit * @min 0 * @max 100 * @decimal 1 @@ -360,8 +360,8 @@ PARAM_DEFINE_FLOAT(DS_ROT_K_PITCH, 30.0f); /** * rudder turn coordination FF-gain - * - * @unit + * + * @unit * @min 0 * @max 100 * @decimal 1 @@ -372,8 +372,8 @@ PARAM_DEFINE_FLOAT(DS_ROT_FF_YAW, 1.0f); /** * normalized damping coefficient of attitude PD-controller (body roll-direction) - * - * @unit + * + * @unit * @min 0 * @max 2 * @decimal 2 @@ -384,8 +384,8 @@ PARAM_DEFINE_FLOAT(DS_ROT_C_ROLL, 1.0f); /** * normalized damping coefficient of attitude PD-controller (body pitch-direction) - * - * @unit + * + * @unit * @min 0 * @max 2 * @decimal 2 @@ -396,8 +396,8 @@ PARAM_DEFINE_FLOAT(DS_ROT_C_PITCH, 1.0f); /** * rudder turn coordination P-gain - * - * @unit + * + * @unit * @min 0 * @max 100 * @decimal 2 @@ -413,8 +413,8 @@ PARAM_DEFINE_FLOAT(DS_ROT_P_YAW, 1.0f); /** * roll gain of K_ACT (actuator deflection gain) - * - * @unit + * + * @unit * @min 0 * @max 100 * @decimal 3 @@ -425,8 +425,8 @@ PARAM_DEFINE_FLOAT(DS_K_ACT_ROLL, 0.25f); /** * pitch gain of K_ACT (actuator deflection gain) - * - * @unit + * + * @unit * @min 0 * @max 100 * @decimal 2 @@ -438,8 +438,8 @@ PARAM_DEFINE_FLOAT(DS_K_ACT_PITCH, 0.05f); /** * roll gain of K_ACT_DAMPING (actuator damping gain) - * - * @unit + * + * @unit * @min 0 * @max 100 * @decimal 3 @@ -450,8 +450,8 @@ PARAM_DEFINE_FLOAT(DS_K_DAMP_ROLL, 0.04f); /** * pitch gain of K_ACT_DAMPING (actuator damping gain) - * - * @unit + * + * @unit * @min 0 * @max 100 * @decimal 3 @@ -467,8 +467,8 @@ PARAM_DEFINE_FLOAT(DS_K_DAMP_PITCH, 0.02f); /** * latitude of trajectory start point (WGS84) - * - * @unit + * + * @unit * @min -90 * @max 90 * @decimal 7 @@ -479,8 +479,8 @@ PARAM_DEFINE_FLOAT(DS_ORIGIN_LAT, 47.3130000f); /** * longitude of trajectory start point (WGS84) - * - * @unit + * + * @unit * @min -180 * @max 180 * @decimal 7 @@ -491,8 +491,8 @@ PARAM_DEFINE_FLOAT(DS_ORIGIN_LON, 8.8100000f); /** * altitude of trajectory start point (WGS84) - * - * @unit + * + * @unit * @min 0 * @max 2000 * @decimal 1 @@ -507,8 +507,8 @@ PARAM_DEFINE_FLOAT(DS_ORIGIN_ALT, 537.0f); /** * integer in {0,1,2,3,4,5} defining the loiter trajectory - * - * @unit + * + * @unit * @min 0 * @max 7 * @decimal 1 @@ -523,7 +523,7 @@ PARAM_DEFINE_INT32(DS_LOITER, 0); /** * integer defining wind heading - * + * * @unit deg * @min -180 * @max 180 @@ -539,8 +539,8 @@ PARAM_DEFINE_INT32(DS_W_HEADING, 0); /** * float defining wind shear vertical postition in soaring frame - * - * @unit + * + * @unit * @min 0 * @max 100 * @decimal 1 @@ -554,15 +554,15 @@ PARAM_DEFINE_FLOAT(DS_W_HEIGHT, 100.f); // ============================================== /** * float in [0,1] corresponding to the engine thrust - * - * @unit + * + * @unit * @min 0 * @max 1 * @decimal 2 * @increment 0.01 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_THRUST, 0); +PARAM_DEFINE_FLOAT(DS_THRUST, 0.7); // ====================================================== // ============= controller force saturation ============ @@ -570,8 +570,8 @@ PARAM_DEFINE_FLOAT(DS_THRUST, 0); /** * integer in {0,1} defining if the commanded force has an upper bound (saturates), 0=no saturation, 1=saturation - * - * @unit + * + * @unit * @min 0 * @max 1 * @decimal 1 @@ -587,7 +587,7 @@ PARAM_DEFINE_INT32(DS_SWITCH_SAT, 1); /** * integer in {0,1} defining if the trajectory origin is taken from hardcoded params or shear estimate, 1=params, 0=estimate - * @unit + * @unit * @min 0 * @max 1 * @decimal 1 @@ -602,7 +602,7 @@ PARAM_DEFINE_INT32(DS_SWITCH_ORI_HC, 1); /** * integer in {0,1} defining if the loiter circle defined by param DS_LOITER shall be used, 0=soaring, 1=loiter - * @unit + * @unit * @min 0 * @max 1 * @decimal 1 @@ -617,14 +617,14 @@ PARAM_DEFINE_INT32(DS_SWITCH_LOITER, 1); /** * integer in {0,1} defining if we are using manual feedthrough, only used in SITL mode - * @unit + * @unit * @min 0 * @max 1 * @decimal 1 * @increment 1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_INT32(DS_SWITCH_MANUAL, 1); +PARAM_DEFINE_INT32(DS_SWITCH_MANUAL, 0); // ===================================================== // ============= open loop / closed loop DS ============ @@ -632,7 +632,7 @@ PARAM_DEFINE_INT32(DS_SWITCH_MANUAL, 1); /** * integer in {0,1} defining if the shear parameters are changed by the estimator while soaring (closed loop). 0=fixed shear, 1=changing shear - * @unit + * @unit * @min 0 * @max 1 * @decimal 1 @@ -647,11 +647,11 @@ PARAM_DEFINE_INT32(DS_SWITCH_CLOOP, 0); /** * integer in {0,1} defining if we are running the controller in SITL. 0=hardware, 1=sitl - * @unit + * @unit * @min 0 * @max 1 * @decimal 1 * @increment 1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_INT32(DS_SWITCH_SITL, 0); \ No newline at end of file +PARAM_DEFINE_INT32(DS_SWITCH_SITL, 0);