From 1b39d5795a93310bc1cdf10120b0a7cb522acbbd Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Wed, 15 Nov 2023 10:24:54 +0100 Subject: [PATCH] Fix format --- .../FixedwingPositionINDIControl.cpp | 2317 +++++++++-------- .../FixedwingPositionINDIControl.hpp | 81 +- .../fw_dyn_soar_control_params.c | 182 +- .../FixedwingShearEstimator.cpp | 838 +++--- .../FixedwingShearEstimator.hpp | 118 +- .../fw_dyn_soar_estimator_params.c | 20 +- 6 files changed, 1840 insertions(+), 1716 deletions(-) diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index 18908f5632..46b55e6743 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -53,12 +53,12 @@ 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)), + _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")) { // limit to 100 Hz - _vehicle_angular_velocity_sub.set_interval_ms(1000.f/_sample_frequency); + _vehicle_angular_velocity_sub.set_interval_ms(1000.f / _sample_frequency); /* fetch initial parameter values */ @@ -77,39 +77,40 @@ FixedwingPositionINDIControl::init() PX4_ERR("vehicle position callback registration failed!"); return false; } - PX4_INFO("Starting FW_DYN_SOAR_CONTROLLER"); - char filename[] = "trajectory0.csv"; - _read_trajectory_coeffs_csv(filename); - // initialize transformations - _R_ned_to_enu *= 0.f; - _R_ned_to_enu(0,1) = 1.f; - _R_ned_to_enu(1,0) = 1.f; - _R_ned_to_enu(2,2) = -1.f; + PX4_INFO("Starting FW_DYN_SOAR_CONTROLLER"); + char filename[] = "trajectory0.csv"; + _read_trajectory_coeffs_csv(filename); + + // initialize transformations + _R_ned_to_enu *= 0.f; + _R_ned_to_enu(0, 1) = 1.f; + _R_ned_to_enu(1, 0) = 1.f; + _R_ned_to_enu(2, 2) = -1.f; _R_ned_to_enu.renormalize(); - _R_enu_to_ned = _R_ned_to_enu; + _R_enu_to_ned = _R_ned_to_enu; - // initialize wind shear params - _shear_v_max = 8.f; - _shear_alpha = 0.5f; - _shear_h_ref = 0.f; - _shear_heading = M_PI_2_F; - _shear_aspd = 20.f; + // initialize wind shear params + _shear_v_max = 8.f; + _shear_alpha = 0.5f; + _shear_h_ref = 0.f; + _shear_heading = M_PI_2_F; + _shear_aspd = 20.f; - // init wind estimates - for (int i=0; i<3; i++) { - _wind_estimate(i) = 0.0f; - _wind_estimate_EKF(i) = 0.0f; - } + // init wind estimates + for (int i = 0; i < 3; i++) { + _wind_estimate(i) = 0.0f; + _wind_estimate_EKF(i) = 0.0f; + } - // initialize in manual feedthrough - _switch_manual = true; + // initialize in manual feedthrough + _switch_manual = true; - // fix sitl mode on startup (no switch at runtime) - _switch_sitl = _param_switch_sitl.get(); + // fix sitl mode on startup (no switch at runtime) + _switch_sitl = _param_switch_sitl.get(); - // initialize transform to trajec frame - _compute_trajectory_transform(); + // initialize transform to trajec frame + _compute_trajectory_transform(); return true; } @@ -120,83 +121,84 @@ FixedwingPositionINDIControl::parameters_update() updateParams(); // INDI parameters - _K_x *= 0.f; - _K_v *= 0.f; - _K_a *= 0.f; - _K_q *= 0.f; - _K_w *= 0.f; - _K_x(0,0) = _param_lin_k_x.get(); - _K_x(1,1) = _param_lin_k_y.get(); - _K_x(2,2) = _param_lin_k_z.get(); - _K_v(0,0) = _param_lin_c_x.get()*2.f*sqrtf(_param_lin_k_x.get()); - _K_v(1,1) = _param_lin_c_y.get()*2.f*sqrtf(_param_lin_k_y.get()); - _K_v(2,2) = _param_lin_c_z.get()*2.f*sqrtf(_param_lin_k_z.get()); - _K_a(0,0) = _param_lin_ff_x.get(); - _K_a(1,1) = _param_lin_ff_y.get(); - _K_a(2,2) = _param_lin_ff_z.get(); - _K_q(0,0) = _param_rot_k_roll.get(); - _K_q(1,1) = _param_rot_k_pitch.get(); - _K_q(2,2) = _param_rot_ff_yaw.get(); // rudder is controlled via turn coordination, not INDI - _K_w(0,0) = _param_rot_c_roll.get()*2.f*sqrtf(_param_rot_k_roll.get()); - _K_w(1,1) = _param_rot_c_pitch.get()*2.f*sqrtf(_param_rot_k_pitch.get()); - _K_w(2,2) = _param_rot_p_yaw.get(); // rudder is controlled via turn coordination, not INDI + _K_x *= 0.f; + _K_v *= 0.f; + _K_a *= 0.f; + _K_q *= 0.f; + _K_w *= 0.f; + _K_x(0, 0) = _param_lin_k_x.get(); + _K_x(1, 1) = _param_lin_k_y.get(); + _K_x(2, 2) = _param_lin_k_z.get(); + _K_v(0, 0) = _param_lin_c_x.get() * 2.f * sqrtf(_param_lin_k_x.get()); + _K_v(1, 1) = _param_lin_c_y.get() * 2.f * sqrtf(_param_lin_k_y.get()); + _K_v(2, 2) = _param_lin_c_z.get() * 2.f * sqrtf(_param_lin_k_z.get()); + _K_a(0, 0) = _param_lin_ff_x.get(); + _K_a(1, 1) = _param_lin_ff_y.get(); + _K_a(2, 2) = _param_lin_ff_z.get(); + _K_q(0, 0) = _param_rot_k_roll.get(); + _K_q(1, 1) = _param_rot_k_pitch.get(); + _K_q(2, 2) = _param_rot_ff_yaw.get(); // rudder is controlled via turn coordination, not INDI + _K_w(0, 0) = _param_rot_c_roll.get() * 2.f * sqrtf(_param_rot_k_roll.get()); + _K_w(1, 1) = _param_rot_c_pitch.get() * 2.f * sqrtf(_param_rot_k_pitch.get()); + _K_w(2, 2) = _param_rot_p_yaw.get(); // rudder is controlled via turn coordination, not INDI - // aircraft parameters - _inertia(0,0) = _param_fw_inertia_roll.get(); - _inertia(1,1) = _param_fw_inertia_pitch.get(); - _inertia(2,2) = _param_fw_inertia_yaw.get(); - _inertia(0,2) = _param_fw_inertia_rp.get(); - _inertia(2,0) = _param_fw_inertia_rp.get(); - _mass = _param_fw_mass.get(); - _area = _param_fw_wing_area.get(); - _rho = _param_rho.get(); - _C_L0 = _param_fw_c_l0.get(); - _C_L1 = _param_fw_c_l1.get(); - _C_D0 = _param_fw_c_d0.get(); - _C_D1 = _param_fw_c_d1.get(); - _C_D2 = _param_fw_c_d2.get(); - _C_B1 = _param_fw_c_b1.get(); - _aoa_offset = _param_aoa_offset.get(); - _stall_speed = _param_stall_speed.get(); + // aircraft parameters + _inertia(0, 0) = _param_fw_inertia_roll.get(); + _inertia(1, 1) = _param_fw_inertia_pitch.get(); + _inertia(2, 2) = _param_fw_inertia_yaw.get(); + _inertia(0, 2) = _param_fw_inertia_rp.get(); + _inertia(2, 0) = _param_fw_inertia_rp.get(); + _mass = _param_fw_mass.get(); + _area = _param_fw_wing_area.get(); + _rho = _param_rho.get(); + _C_L0 = _param_fw_c_l0.get(); + _C_L1 = _param_fw_c_l1.get(); + _C_D0 = _param_fw_c_d0.get(); + _C_D1 = _param_fw_c_d1.get(); + _C_D2 = _param_fw_c_d2.get(); + _C_B1 = _param_fw_c_b1.get(); + _aoa_offset = _param_aoa_offset.get(); + _stall_speed = _param_stall_speed.get(); - // actuator gains - _k_ail = _param_k_act_roll.get(); - _k_ele = _param_k_act_pitch.get(); - _k_d_roll = _param_k_damping_roll.get(); - _k_d_pitch = _param_k_damping_pitch.get(); + // actuator gains + _k_ail = _param_k_act_roll.get(); + _k_ele = _param_k_act_pitch.get(); + _k_d_roll = _param_k_damping_roll.get(); + _k_d_pitch = _param_k_damping_pitch.get(); - // trajectory origin - _origin_lat = _param_origin_lat.get(); - _origin_lon = _param_origin_lon.get(); - _origin_alt = _param_origin_alt.get(); - // 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); - _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(); - _switch_origin_hardcoded = _param_switch_origin_hardcoded.get(); - _switch_cl_soaring = _param_switch_cloop.get(); - if (_switch_sitl) { - // only use switch manual param in sitl mode - _switch_manual = _param_switch_manual.get(); - } + // trajectory origin + _origin_lat = _param_origin_lat.get(); + _origin_lon = _param_origin_lon.get(); + _origin_alt = _param_origin_alt.get(); + // 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); + _origin_D = _local_pos.ref_alt - _origin_alt; + PX4_INFO("local reference frame updated"); - _loiter = _param_loiter.get(); + _thrust_pos = _param_thrust.get(); + _thrust = _thrust_pos; + _switch_saturation = _param_switch_saturation.get(); + _switch_origin_hardcoded = _param_switch_origin_hardcoded.get(); + _switch_cl_soaring = _param_switch_cloop.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(); - _select_loiter_trajectory(); - } + if (_switch_sitl) { + // only use switch manual param in sitl mode + _switch_manual = _param_switch_manual.get(); + } + + _loiter = _param_loiter.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(); + _select_loiter_trajectory(); + } return PX4_OK; @@ -218,7 +220,7 @@ FixedwingPositionINDIControl::airspeed_poll() _airspeed_last_valid = airspeed_validated.timestamp; _true_airspeed = airspeed_validated.true_airspeed_m_s; - _cal_airspeed = airspeed_validated.calibrated_airspeed_m_s; + _cal_airspeed = airspeed_validated.calibrated_airspeed_m_s; } } else { @@ -227,7 +229,8 @@ FixedwingPositionINDIControl::airspeed_poll() airspeed_valid = false; } } - _airspeed_valid = airspeed_valid; + + _airspeed_valid = airspeed_valid; } void @@ -253,7 +256,8 @@ FixedwingPositionINDIControl::airflow_aoa_poll() aoa_valid = false; } } - _aoa_valid = aoa_valid; + + _aoa_valid = aoa_valid; } void @@ -279,69 +283,75 @@ FixedwingPositionINDIControl::airflow_slip_poll() slip_valid = false; } } - _slip_valid = slip_valid; + + _slip_valid = slip_valid; } void FixedwingPositionINDIControl::vehicle_status_poll() { - if(_vehicle_status_sub.update(&_vehicle_status)){ - //print_message(_vehicle_status); - } + if (_vehicle_status_sub.update(&_vehicle_status)) { + //print_message(_vehicle_status); + } } void FixedwingPositionINDIControl::manual_control_setpoint_poll() { - if(_switch_manual){ - _manual_control_setpoint_sub.update(&_manual_control_setpoint); - _thrust = _manual_control_setpoint.z; - } - else { - _thrust = _thrust_pos; - } + if (_switch_manual) { + _manual_control_setpoint_sub.update(&_manual_control_setpoint); + _thrust = _manual_control_setpoint.z; + + } else { + _thrust = _thrust_pos; + } } void FixedwingPositionINDIControl::rc_channels_poll() { - if (_rc_channels_sub.update(&_rc_channels)) { - // use flaps channel to select manual feedthrough - if (!_switch_sitl) { - if (_rc_channels.channels[5]>=0.f){ - _switch_manual = true; - } - else{ - _switch_manual = false; - } - } - } + if (_rc_channels_sub.update(&_rc_channels)) { + // use flaps channel to select manual feedthrough + if (!_switch_sitl) { + if (_rc_channels.channels[5] >= 0.f) { + _switch_manual = true; + + } else { + _switch_manual = false; + } + } + } } void FixedwingPositionINDIControl::vehicle_attitude_poll() { if (_vehicle_attitude_sub.update(&_attitude)) { - // get rotation between NED frames - Dcmf R_ned_frd(Quatf(_attitude.q)); - // get rotation from FRD to ENU frame (change of basis) - Dcmf R_enu_frd(_R_ned_to_enu*R_ned_frd); + // get rotation between NED frames + Dcmf R_ned_frd(Quatf(_attitude.q)); + // get rotation from FRD to ENU frame (change of basis) + Dcmf R_enu_frd(_R_ned_to_enu * R_ned_frd); _att = Quatf(R_enu_frd); - } - if(hrt_absolute_time()-_attitude.timestamp > 20_ms && _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD){ - PX4_ERR("attitude sample is too old"); - } + } + + if (hrt_absolute_time() - _attitude.timestamp > 20_ms + && _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) { + PX4_ERR("attitude sample is too old"); + } } void 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); - 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"); - } +{ + // + // no need to check if it was updated as the main loop is fired based on an update... + // + _omega = Vector3f(_angular_vel.xyz); + + 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 @@ -349,445 +359,471 @@ 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"); - } + } + + 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() { - //vehicle_local_position_s pos; - if (_vehicle_local_position_sub.update(&_local_pos)){ - _pos = _R_ned_to_enu*Vector3f{_local_pos.x,_local_pos.y,_local_pos.z}; - _vel = _R_ned_to_enu*Vector3f{_local_pos.vx,_local_pos.vy,_local_pos.vz}; - // take accel from faster message, since 50Hz is too slow... - // transform to soaring frame - _pos = _pos - _R_ned_to_enu * Vector3f{_origin_N, _origin_E, _origin_D}; - } - if(hrt_absolute_time()-_local_pos.timestamp > 50_ms && _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD){ - PX4_ERR("local position sample is too old"); - } + //vehicle_local_position_s pos; + if (_vehicle_local_position_sub.update(&_local_pos)) { + _pos = _R_ned_to_enu * Vector3f{_local_pos.x, _local_pos.y, _local_pos.z}; + _vel = _R_ned_to_enu * Vector3f{_local_pos.vx, _local_pos.vy, _local_pos.vz}; + // take accel from faster message, since 50Hz is too slow... + // transform to soaring frame + _pos = _pos - _R_ned_to_enu * Vector3f{_origin_N, _origin_E, _origin_D}; + } + + if (hrt_absolute_time() - _local_pos.timestamp > 50_ms + && _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) { + PX4_ERR("local position sample is too old"); + } } void FixedwingPositionINDIControl::vehicle_acceleration_poll() { - //vehicle_local_position_s pos; - if (_vehicle_acceleration_sub.update(&_acceleration)){ - Dcmf R_ib(_att); - _acc = R_ib*Vector3f(_acceleration.xyz) - Vector3f{0.f,0.f,9.81f}; - } - if(hrt_absolute_time()-_acceleration.timestamp > 20_ms && _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD){ - PX4_ERR("linear acceleration sample is too old"); - } + //vehicle_local_position_s pos; + if (_vehicle_acceleration_sub.update(&_acceleration)) { + Dcmf R_ib(_att); + _acc = R_ib * Vector3f(_acceleration.xyz) - Vector3f{0.f, 0.f, 9.81f}; + } + + if (hrt_absolute_time() - _acceleration.timestamp > 20_ms + && _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) { + PX4_ERR("linear acceleration sample is too old"); + } } void FixedwingPositionINDIControl::actuator_controls_poll() { - _actuator_controls_sub.update(&_actuators); + _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"); - } - } + 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; - } - - } + 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() { - Eulerf e(0.f, 0.f, _shear_heading); - _R_enu_to_trajec = Dcmf(e); - _R_trajec_to_enu = _R_enu_to_trajec.transpose(); - _vec_enu_to_trajec = Vector3f{0.f,0.f,_shear_h_ref}; + Eulerf e(0.f, 0.f, _shear_heading); + _R_enu_to_trajec = Dcmf(e); + _R_trajec_to_enu = _R_enu_to_trajec.transpose(); + _vec_enu_to_trajec = Vector3f{0.f, 0.f, _shear_h_ref}; } Vector3f FixedwingPositionINDIControl::_compute_wind_estimate() { - Dcmf R_ib(_att); - Dcmf R_bi(R_ib.transpose()); - // compute expected AoA from g-forces: - Vector3f body_force = _mass*R_bi*(_acc + Vector3f{0.f,0.f,9.81f}); + Dcmf R_ib(_att); + Dcmf R_bi(R_ib.transpose()); + // compute expected AoA from g-forces: + Vector3f body_force = _mass * R_bi * (_acc + Vector3f{0.f, 0.f, 9.81f}); - // approximate lift force, since implicit equation cannot be solved analytically: - // since alpha<<1, we approximate the lift force L = sin(alpha)*Fx - cos(alpha)*Fz - // 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) / - (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); - float u_approx = _true_airspeed; - float v_approx = body_force(1)*_true_airspeed / (0.5f*_rho*powf(speed,2)*_area*_C_B1); - float w_approx = tanf(AoA_approx-_aoa_offset)*_true_airspeed; - Vector3f vel_air = R_ib*(Vector3f{u_approx, v_approx, w_approx}); + // approximate lift force, since implicit equation cannot be solved analytically: + // since alpha<<1, we approximate the lift force L = sin(alpha)*Fx - cos(alpha)*Fz + // 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) / + (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); + float u_approx = _true_airspeed; + float v_approx = body_force(1) * _true_airspeed / (0.5f * _rho * powf(speed, 2) * _area * _C_B1); + float w_approx = tanf(AoA_approx - _aoa_offset) * _true_airspeed; + Vector3f vel_air = R_ib * (Vector3f{u_approx, v_approx, w_approx}); - // compute wind from wind triangle - Vector3f wind = _vel - vel_air; - //PX4_INFO("wind estimate: \t%.1f, \t%.1f, \t%.1f", (double)wind(0), (double)wind(1), (double)wind(2)); - return wind; + // compute wind from wind triangle + Vector3f wind = _vel - vel_air; + //PX4_INFO("wind estimate: \t%.1f, \t%.1f, \t%.1f", (double)wind(0), (double)wind(1), (double)wind(2)); + return wind; } Vector3f FixedwingPositionINDIControl::_compute_wind_estimate_EKF() { - Dcmf R_ib(_att); - Dcmf R_bi(R_ib.transpose()); - // compute expected AoA from g-forces: - Vector3f body_force = _mass*R_bi*(_acc + Vector3f{0.f,0.f,9.81f}); + Dcmf R_ib(_att); + Dcmf R_bi(R_ib.transpose()); + // compute expected AoA from g-forces: + Vector3f body_force = _mass * R_bi * (_acc + Vector3f{0.f, 0.f, 9.81f}); - // ***************** NEW COMPUTATION FROM MATLAB CALIBRATION ********************** - float speed = fmaxf(_cal_airspeed, _stall_speed); - float u_approx = _true_airspeed; - float v_approx = body_force(1)*_true_airspeed / (0.5f*_rho*powf(speed,2)*_area*_C_B1); - float w_approx = (-body_force(2)*_true_airspeed / (0.5f*_rho*powf(speed,2)*_area)-0.1949f)/3.5928f; - Vector3f vel_air = R_ib*(Vector3f{u_approx, v_approx, w_approx}); + // ***************** NEW COMPUTATION FROM MATLAB CALIBRATION ********************** + float speed = fmaxf(_cal_airspeed, _stall_speed); + float u_approx = _true_airspeed; + float v_approx = body_force(1) * _true_airspeed / (0.5f * _rho * powf(speed, 2) * _area * _C_B1); + float w_approx = (-body_force(2) * _true_airspeed / (0.5f * _rho * powf(speed, 2) * _area) - 0.1949f) / 3.5928f; + Vector3f vel_air = R_ib * (Vector3f{u_approx, v_approx, w_approx}); - // compute wind from wind triangle - Vector3f wind = _vel - vel_air; - //PX4_INFO("wind estimate: \t%.1f, \t%.1f, \t%.1f", (double)wind(0), (double)wind(1), (double)wind(2)); - return wind; + // compute wind from wind triangle + Vector3f wind = _vel - vel_air; + //PX4_INFO("wind estimate: \t%.1f, \t%.1f, \t%.1f", (double)wind(0), (double)wind(1), (double)wind(2)); + return wind; } 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)); - wind(2) = _lp_filter_wind[2].apply(wind(2)); - _wind_estimate = wind; - return; +{ + // apply some filtering + wind(0) = _lp_filter_wind[0].apply(wind(0)); + wind(1) = _lp_filter_wind[1].apply(wind(1)); + wind(2) = _lp_filter_wind[2].apply(wind(2)); + _wind_estimate = wind; + return; } 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)); - wind(2) = _lp_filter_wind_EKF[2].apply(wind(2)); - _wind_estimate_EKF = wind; - return; -} - - -void -FixedwingPositionINDIControl::_reverse(char* str, int len) { - // copied from: https://www.geeksforgeeks.org/convert-floating-point-number-string/ - int i = 0, j = len - 1, temp; - while (i < j) { - temp = str[i]; - str[i] = str[j]; - str[j] = temp; - i++; - j--; - } + // apply some filtering + wind(0) = _lp_filter_wind_EKF[0].apply(wind(0)); + wind(1) = _lp_filter_wind_EKF[1].apply(wind(1)); + wind(2) = _lp_filter_wind_EKF[2].apply(wind(2)); + _wind_estimate_EKF = wind; + return; } -int + +void +FixedwingPositionINDIControl::_reverse(char *str, int len) +{ + // copied from: https://www.geeksforgeeks.org/convert-floating-point-number-string/ + int i = 0, j = len - 1, temp; + + while (i < j) { + temp = str[i]; + str[i] = str[j]; + str[j] = temp; + i++; + j--; + } +} + +int FixedwingPositionINDIControl::_int_to_str(int x, char str[], int d) { - // copied from: https://www.geeksforgeeks.org/convert-floating-point-number-string/ - int i = 0; - while (x) { - 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; + // copied from: https://www.geeksforgeeks.org/convert-floating-point-number-string/ + int i = 0; + + while (x) { + 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; } void -FixedwingPositionINDIControl::_float_to_str(float n, char* res, int afterpoint) +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); - } + // 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); + } } void FixedwingPositionINDIControl::_select_loiter_trajectory() { - // select loiter trajectory for loiter test - char filename[16]; - switch (_loiter) - { - case 0: - strcpy(filename,"trajectory0.csv"); - break; - case 1: - strcpy(filename,"trajectory1.csv"); - break; - case 2: - strcpy(filename,"trajectory2.csv"); - break; - case 3: - strcpy(filename,"trajectory3.csv"); - break; - case 4: - strcpy(filename,"trajectory4.csv"); - break; - case 5: - strcpy(filename,"trajectory5.csv"); - break; - case 6: - strcpy(filename,"trajectory6.csv"); - break; - case 7: - strcpy(filename,"trajectory7.csv"); - break; + // select loiter trajectory for loiter test + char filename[16]; - default: - strcpy(filename,"trajectory0.csv"); - } + switch (_loiter) { + case 0: + strcpy(filename, "trajectory0.csv"); + break; - /* - 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, - 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); + case 1: + strcpy(filename, "trajectory1.csv"); + break; + + case 2: + strcpy(filename, "trajectory2.csv"); + break; + + case 3: + strcpy(filename, "trajectory3.csv"); + break; + + case 4: + strcpy(filename, "trajectory4.csv"); + break; + + case 5: + strcpy(filename, "trajectory5.csv"); + break; + + case 6: + strcpy(filename, "trajectory6.csv"); + break; + + case 7: + strcpy(filename, "trajectory7.csv"); + break; + + default: + strcpy(filename, "trajectory0.csv"); + } + + /* + 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, + 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 - in {nominal, robust} - in [08,12] - in [020, 100] - in [E_min, E_max] - */ + /* + 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). - char file[40] = "robust/coeffs_robust"; - char v_str[3]; - char a_str[3]; - char e_str[3]; - // get the correct filename - _float_to_str(_shear_v_max, v_str, 0); - _float_to_str(_shear_alpha*100.f, a_str, 0); - _float_to_str(_shear_aspd, e_str, 0); + The filename structure of the trajectories is the following: + trajec_____ + with + in {nominal, robust} + in [08,12] + in [020, 100] + in [E_min, E_max] + */ - strcat(file,"_"); - strcat(file,v_str); - strcat(file,"_"); - strcat(file,a_str); - strcat(file,"_"); - strcat(file,e_str); - strcat(file,".csv"); - PX4_INFO("filename: \t%.40s", file); - // get the basis coeffs from file - _read_trajectory_coeffs_csv(file); + char file[40] = "robust/coeffs_robust"; + char v_str[3]; + char a_str[3]; + char e_str[3]; + // get the correct filename + _float_to_str(_shear_v_max, v_str, 0); + _float_to_str(_shear_alpha * 100.f, a_str, 0); + _float_to_str(_shear_aspd, e_str, 0); + + strcat(file, "_"); + strcat(file, v_str); + strcat(file, "_"); + strcat(file, a_str); + strcat(file, "_"); + strcat(file, e_str); + strcat(file, ".csv"); + PX4_INFO("filename: \t%.40s", file); + // get the basis coeffs from file + _read_trajectory_coeffs_csv(file); } void FixedwingPositionINDIControl::_read_trajectory_coeffs_csv(char *filename) { - // ======================================================================= - bool error = false; + // ======================================================================= + 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/"; - //PX4_ERR(home_dir); - strcat(home_dir,filename); - FILE* fp = fopen(home_dir, "r"); + //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"); - if (fp == nullptr) { - PX4_ERR("Can't open file"); - error = true; - } - else { - // Here we have taken size of - // array 1024 you can modify it - const uint buffersize = _num_basis_funs*32; - char buffer[buffersize]; + if (fp == nullptr) { + PX4_ERR("Can't open file"); + error = true; - int row = 0; - int column = 0; + } else { + // Here we have taken size of + // array 1024 you can modify it + const uint buffersize = _num_basis_funs * 32; + char buffer[buffersize]; - // loop over rows - while (fgets(buffer, - buffersize, fp)) { - column = 0; - - // Splitting the data - char* value = strtok(buffer, ","); - - // loop over columns - while (value) { - if (*value=='\0'||*value==' ') { - // simply skip extra characters - continue; - } - switch(row){ - case 0: - _basis_coeffs_x(column) = (float)atof(value); - break; - case 1: - _basis_coeffs_y(column) = (float)atof(value); - break; - case 2: - _basis_coeffs_z(column) = (float)atof(value); - break; + int row = 0; + int column = 0; - default: - break; - } - //PX4_INFO("row: %d, col: %d, read value: %.3f", row, column, (double)atof(value)); - value = strtok(NULL, ","); - column++; - - } - row++; - } - int failure = fclose(fp); - if (failure==-1) { - PX4_ERR("Can't close file"); - } - } - // ======================================================================= + // loop over rows + while (fgets(buffer, + buffersize, fp)) { + column = 0; + + // Splitting the data + char *value = strtok(buffer, ","); + + // loop over columns + while (value) { + if (*value == '\0' || *value == ' ') { + // simply skip extra characters + continue; + } + + switch (row) { + case 0: + _basis_coeffs_x(column) = (float)atof(value); + break; + + case 1: + _basis_coeffs_y(column) = (float)atof(value); + break; + + case 2: + _basis_coeffs_z(column) = (float)atof(value); + break; + + default: + break; + } + + //PX4_INFO("row: %d, col: %d, read value: %.3f", row, column, (double)atof(value)); + value = strtok(NULL, ","); + column++; + + } + + row++; + } + + int failure = fclose(fp); + + if (failure == -1) { + PX4_ERR("Can't close file"); + } + } + + // ======================================================================= - // go back to safety mode loiter circle in 30m height - if(error){ - // 100m radius circle trajec - _basis_coeffs_x(0) = 0.000038f; - _basis_coeffs_x(1) = 1812.140143f; - _basis_coeffs_x(2) = -6365.976106f; - _basis_coeffs_x(3) = 10773.875378f; - _basis_coeffs_x(4) = -9441.287977f; - _basis_coeffs_x(5) = 1439.744061f; - _basis_coeffs_x(6) = 6853.112823f; - _basis_coeffs_x(7) = -7433.361925f; - _basis_coeffs_x(8) = -72.566660f; - _basis_coeffs_x(9) = 7518.521784f; - _basis_coeffs_x(10) = -6807.858677f; - _basis_coeffs_x(11) = -1586.021605f; - _basis_coeffs_x(12) = 9599.405711f; - _basis_coeffs_x(13) = -10876.256865f; - _basis_coeffs_x(14) = 6406.017620f; - _basis_coeffs_x(15) = -1819.600542f; + // go back to safety mode loiter circle in 30m height + if (error) { + // 100m radius circle trajec + _basis_coeffs_x(0) = 0.000038f; + _basis_coeffs_x(1) = 1812.140143f; + _basis_coeffs_x(2) = -6365.976106f; + _basis_coeffs_x(3) = 10773.875378f; + _basis_coeffs_x(4) = -9441.287977f; + _basis_coeffs_x(5) = 1439.744061f; + _basis_coeffs_x(6) = 6853.112823f; + _basis_coeffs_x(7) = -7433.361925f; + _basis_coeffs_x(8) = -72.566660f; + _basis_coeffs_x(9) = 7518.521784f; + _basis_coeffs_x(10) = -6807.858677f; + _basis_coeffs_x(11) = -1586.021605f; + _basis_coeffs_x(12) = 9599.405711f; + _basis_coeffs_x(13) = -10876.256865f; + _basis_coeffs_x(14) = 6406.017620f; + _basis_coeffs_x(15) = -1819.600542f; - _basis_coeffs_y(0) = -59.999852f; - _basis_coeffs_y(1) = -2811.660383f; - _basis_coeffs_y(2) = 13178.399227f; - _basis_coeffs_y(3) = -30339.925641f; - _basis_coeffs_y(4) = 43145.286828f; - _basis_coeffs_y(5) = -37009.839292f; - _basis_coeffs_y(6) = 9438.328009f; - _basis_coeffs_y(7) = 23631.637452f; - _basis_coeffs_y(8) = -38371.559953f; - _basis_coeffs_y(9) = 23715.306334f; - _basis_coeffs_y(10) = 9316.038368f; - _basis_coeffs_y(11) = -36903.451639f; - _basis_coeffs_y(12) = 43082.749551f; - _basis_coeffs_y(13) = -30315.482005f; - _basis_coeffs_y(14) = 13172.915247f; - _basis_coeffs_y(15) = -2811.186858f; + _basis_coeffs_y(0) = -59.999852f; + _basis_coeffs_y(1) = -2811.660383f; + _basis_coeffs_y(2) = 13178.399227f; + _basis_coeffs_y(3) = -30339.925641f; + _basis_coeffs_y(4) = 43145.286828f; + _basis_coeffs_y(5) = -37009.839292f; + _basis_coeffs_y(6) = 9438.328009f; + _basis_coeffs_y(7) = 23631.637452f; + _basis_coeffs_y(8) = -38371.559953f; + _basis_coeffs_y(9) = 23715.306334f; + _basis_coeffs_y(10) = 9316.038368f; + _basis_coeffs_y(11) = -36903.451639f; + _basis_coeffs_y(12) = 43082.749551f; + _basis_coeffs_y(13) = -30315.482005f; + _basis_coeffs_y(14) = 13172.915247f; + _basis_coeffs_y(15) = -2811.186858f; - _basis_coeffs_z(0) = 30.0f; - _basis_coeffs_z(1) = 0.0f; - _basis_coeffs_z(2) = 0.0f; - _basis_coeffs_z(3) = 0.0f; - _basis_coeffs_z(4) = 0.0f; - _basis_coeffs_z(5) = 0.0f; - _basis_coeffs_z(6) = 0.0f; - _basis_coeffs_z(7) = 0.0f; - _basis_coeffs_z(8) = 0.0f; - _basis_coeffs_z(9) = 0.0f; - _basis_coeffs_z(10) = 0.0f; - _basis_coeffs_z(11) = 0.0f; - _basis_coeffs_z(12) = 0.0f; - _basis_coeffs_z(13) = 0.0f; - _basis_coeffs_z(14) = 0.0f; - _basis_coeffs_z(15) = 0.0f; - } + _basis_coeffs_z(0) = 30.0f; + _basis_coeffs_z(1) = 0.0f; + _basis_coeffs_z(2) = 0.0f; + _basis_coeffs_z(3) = 0.0f; + _basis_coeffs_z(4) = 0.0f; + _basis_coeffs_z(5) = 0.0f; + _basis_coeffs_z(6) = 0.0f; + _basis_coeffs_z(7) = 0.0f; + _basis_coeffs_z(8) = 0.0f; + _basis_coeffs_z(9) = 0.0f; + _basis_coeffs_z(10) = 0.0f; + _basis_coeffs_z(11) = 0.0f; + _basis_coeffs_z(12) = 0.0f; + _basis_coeffs_z(13) = 0.0f; + _basis_coeffs_z(14) = 0.0f; + _basis_coeffs_z(15) = 0.0f; + } } void FixedwingPositionINDIControl::Run() { - if (should_exit()) { + if (should_exit()) { _vehicle_angular_velocity_sub.unregisterCallback(); exit_and_cleanup(); return; } - perf_begin(_loop_perf); + perf_begin(_loop_perf); - // only run controller if pos, vel, acc changed - if (_vehicle_angular_velocity_sub.update(&_angular_vel)) - { - // only update parameters if they changed + // 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(); // check for parameter updates @@ -804,774 +840,815 @@ FixedwingPositionINDIControl::Run() //const float dt = math::constrain((pos.timestamp - _last_run) * 1e-6f, 0.002f, 0.04f); //_last_run = _local_pos.timestamp; - // 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) + // 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) || (_local_pos.xy_reset_counter != _pos_reset_counter) - || (_local_pos.z_reset_counter != _alt_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); - _origin_D = _local_pos.ref_alt - _origin_alt; - PX4_INFO("local reference frame updated"); - } - // update reset counters - _pos_reset_counter = _local_pos.xy_reset_counter; - _alt_reset_counter = _local_pos.z_reset_counter; + || (_local_pos.z_reset_counter != _alt_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); + _origin_D = _local_pos.ref_alt - _origin_alt; + PX4_INFO("local reference frame updated"); + } - // 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 reset counters + _pos_reset_counter = _local_pos.xy_reset_counter; + _alt_reset_counter = _local_pos.z_reset_counter; - // update the shear estimate, only target airspeed is updated in soaring mode - soaring_estimator_shear_poll(); + // 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 transform from trajectory frame to ENU frame (soaring frame) - _compute_trajectory_transform(); + // update the shear estimate, only target airspeed is updated in soaring mode + soaring_estimator_shear_poll(); - // =============================== - // compute wind pseudo-measurement - // =============================== - Vector3f wind = _compute_wind_estimate(); - _set_wind_estimate(wind); - Vector3f wind_EKF = _compute_wind_estimate_EKF(); - _set_wind_estimate_EKF(wind_EKF); + // update transform from trajectory frame to ENU frame (soaring frame) + _compute_trajectory_transform(); + + // =============================== + // compute wind pseudo-measurement + // =============================== + Vector3f wind = _compute_wind_estimate(); + _set_wind_estimate(wind); + Vector3f wind_EKF = _compute_wind_estimate_EKF(); + _set_wind_estimate_EKF(wind_EKF); - // only run actuators poll, when our module is not publishing: - if (_vehicle_status.nav_state != vehicle_status_s::NAVIGATION_STATE_OFFBOARD) { - actuator_controls_poll(); - } + // only run actuators poll, when our module is not publishing: + if (_vehicle_status.nav_state != vehicle_status_s::NAVIGATION_STATE_OFFBOARD) { + actuator_controls_poll(); + } - // ================================= - // get reference point on trajectory - // ================================= - float t_ref = _get_closest_t(_pos); + // ================================= + // get reference point on trajectory + // ================================= + float t_ref = _get_closest_t(_pos); - // ================================================================= - // possibly select a new trajectory, if we are finishing the old one - // ================================================================= - if (!_switch_origin_hardcoded && t_ref>=0.97f && (hrt_absolute_time()-_last_time_trajec)>1000000) { - _select_soaring_trajectory(); - _last_time_trajec = hrt_absolute_time(); - } + // ================================================================= + // possibly select a new trajectory, if we are finishing the old one + // ================================================================= + if (!_switch_origin_hardcoded && t_ref >= 0.97f && (hrt_absolute_time() - _last_time_trajec) > 1000000) { + _select_soaring_trajectory(); + _last_time_trajec = hrt_absolute_time(); + } - // ============================ - // compute reference kinematics - // ============================ - // 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)); - Vector3f pos_ref = _get_position_ref(t_ref); // in inertial ENU - Vector3f vel_ref = _get_velocity_ref(t_ref,T); // in inertial ENU - Vector3f acc_ref = _get_acceleration_ref(t_ref,T); // gravity-corrected acceleration (ENU) - Quatf q = _get_attitude_ref(t_ref,T); - Vector3f omega_ref = _get_angular_velocity_ref(t_ref,T); // body angular velocity - Vector3f alpha_ref = _get_angular_acceleration_ref(t_ref,T); // body angular acceleration + // ============================ + // compute reference kinematics + // ============================ + // 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)); + Vector3f pos_ref = _get_position_ref(t_ref); // in inertial ENU + Vector3f vel_ref = _get_velocity_ref(t_ref, T); // in inertial ENU + Vector3f acc_ref = _get_acceleration_ref(t_ref, T); // gravity-corrected acceleration (ENU) + Quatf q = _get_attitude_ref(t_ref, T); + Vector3f omega_ref = _get_angular_velocity_ref(t_ref, T); // body angular velocity + Vector3f alpha_ref = _get_angular_acceleration_ref(t_ref, T); // body angular acceleration - // ===================== - // compute control input - // ===================== - Vector3f ctrl = _compute_INDI_stage_1(pos_ref, vel_ref, acc_ref, omega_ref, alpha_ref); - Vector3f ctrl1 = _compute_INDI_stage_2(ctrl); + // ===================== + // compute control input + // ===================== + Vector3f ctrl = _compute_INDI_stage_1(pos_ref, vel_ref, acc_ref, omega_ref, alpha_ref); + Vector3f ctrl1 = _compute_INDI_stage_2(ctrl); - // ============================ - // compute actuator deflections - // ============================ - Vector3f ctrl2 = _compute_actuator_deflections(ctrl1); + // ============================ + // compute actuator deflections + // ============================ + Vector3f ctrl2 = _compute_actuator_deflections(ctrl1); - // ================================= - // publish offboard control commands - // ================================= - offboard_control_mode_s ocm{}; - ocm.actuator = true; - ocm.timestamp = hrt_absolute_time(); - _offboard_control_mode_pub.publish(ocm); + // ================================= + // publish offboard control commands + // ================================= + offboard_control_mode_s ocm{}; + ocm.actuator = true; + ocm.timestamp = hrt_absolute_time(); + _offboard_control_mode_pub.publish(ocm); - // Publish actuator controls only once in OFFBOARD + // 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 controller position in ENU frame + // ======================================== + _soaring_controller_position.timestamp = hrt_absolute_time(); - // ===================== - // publish control input - // ===================== - //_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); - _angular_accel_sp.xyz[1] = ctrl(1); - _angular_accel_sp.xyz[2] = ctrl(2); - _angular_accel_sp_pub.publish(_angular_accel_sp); + 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); + } - // ========================= - // publish attitude setpoint - // ========================= - //_attitude_sp = {}; - Quatf q_sp(_R_enu_to_ned*Dcmf(q)); - _attitude_sp.timestamp = hrt_absolute_time(); - _attitude_sp.q_d[0] = q_sp(0); - _attitude_sp.q_d[1] = q_sp(1); - _attitude_sp.q_d[2] = q_sp(2); - _attitude_sp.q_d[3] = q_sp(3); - _attitude_sp_pub.publish(_attitude_sp); + _soaring_controller_position_pub.publish(_soaring_controller_position); - // ====================== - // publish rates setpoint - // ====================== - //_angular_vel_sp = {}; - _angular_vel_sp.timestamp = hrt_absolute_time(); - _angular_vel_sp.roll = omega_ref(0); - _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.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); - //print_message(_actuators); + // ==================================== + // publish controller position setpoint + // ==================================== + _soaring_controller_position_setpoint.timestamp = hrt_absolute_time(); - // ===================== - // 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); + 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 + // ===================== + //_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); + _angular_accel_sp.xyz[1] = ctrl(1); + _angular_accel_sp.xyz[2] = ctrl(2); + _angular_accel_sp_pub.publish(_angular_accel_sp); + + // ========================= + // publish attitude setpoint + // ========================= + //_attitude_sp = {}; + Quatf q_sp(_R_enu_to_ned * Dcmf(q)); + _attitude_sp.timestamp = hrt_absolute_time(); + _attitude_sp.q_d[0] = q_sp(0); + _attitude_sp.q_d[1] = q_sp(1); + _attitude_sp.q_d[2] = q_sp(2); + _attitude_sp.q_d[3] = q_sp(3); + _attitude_sp_pub.publish(_attitude_sp); + + // ====================== + // publish rates setpoint + // ====================== + //_angular_vel_sp = {}; + _angular_vel_sp.timestamp = hrt_absolute_time(); + _angular_vel_sp.roll = omega_ref(0); + _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.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); + //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; - //PX4_INFO("Feedthrough switch: \t%.2f", (double)(_rc_channels.channels[5])); - //PX4_INFO("frequency: \t%.3f", (double)(1000000*100)/(hrt_absolute_time()-_last_time)); - _last_time = hrt_absolute_time(); - } - else { - _counter += 1; - } - } + if (_counter == 100) { + _counter = 0; + //PX4_INFO("Feedthrough switch: \t%.2f", (double)(_rc_channels.channels[5])); + //PX4_INFO("frequency: \t%.3f", (double)(1000000*100)/(hrt_absolute_time()-_last_time)); + _last_time = hrt_absolute_time(); - // =========================== - // publish rate control status - // =========================== - rate_ctrl_status_s rate_ctrl_status{}; - rate_ctrl_status.timestamp = hrt_absolute_time(); - rate_ctrl_status.rollspeed_integ = 0.0f; - rate_ctrl_status.pitchspeed_integ = 0.0f; - rate_ctrl_status.yawspeed_integ = 0.0f; - _rate_ctrl_status_pub.publish(rate_ctrl_status); + } else { + _counter += 1; + } + } - // ============================== - // 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 rate control status + // =========================== + rate_ctrl_status_s rate_ctrl_status{}; + rate_ctrl_status.timestamp = hrt_absolute_time(); + rate_ctrl_status.rollspeed_integ = 0.0f; + rate_ctrl_status.pitchspeed_integ = 0.0f; + rate_ctrl_status.yawspeed_integ = 0.0f; + _rate_ctrl_status_pub.publish(rate_ctrl_status); - // ==================== - // publish debug values - // ==================== - Dcmf R_ib(_att); - Dcmf R_bi(R_ib.transpose()); - Vector3f vel_body = R_bi*(_vel - _wind_estimate); - _slip = atan2f(vel_body(1), vel_body(0))*180.f/M_PI_2_F; - _debug_value.timestamp = hrt_absolute_time(); - _debug_value.value = _slip; - _debug_value_pub.publish(_debug_value); + // ============================== + // 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 + // ==================== + Dcmf R_ib(_att); + Dcmf R_bi(R_ib.transpose()); + Vector3f vel_body = R_bi * (_vel - _wind_estimate); + _slip = atan2f(vel_body(1), vel_body(0)) * 180.f / M_PI_2_F; + _debug_value.timestamp = hrt_absolute_time(); + _debug_value.value = _slip; + _debug_value_pub.publish(_debug_value); + + perf_end(_loop_perf); + } - perf_end(_loop_perf); - } - } Vector FixedwingPositionINDIControl::_get_basis_funs(float t) { - Vector vec; - vec(0) = 1.0f; - float sigma = 1.0f/_num_basis_funs; - for(uint i=1; i<_num_basis_funs; i++){ - float fun1 = sinf(M_PI_F*t); - float fun2 = exp(-powf((t-float(i)/float(_num_basis_funs)),2)/sigma); - vec(i) = fun1*fun2; - } - return vec; + Vector vec; + vec(0) = 1.0f; + float sigma = 1.0f / _num_basis_funs; + + for (uint i = 1; i < _num_basis_funs; i++) { + float fun1 = sinf(M_PI_F * t); + float fun2 = exp(-powf((t - float(i) / float(_num_basis_funs)), 2) / sigma); + vec(i) = fun1 * fun2; + } + + return vec; } Vector FixedwingPositionINDIControl::_get_d_dt_basis_funs(float t) { - Vector vec; - vec(0) = 0.0f; - float sigma = 1.0f/_num_basis_funs; - for(uint i=1; i<_num_basis_funs; i++){ - float fun1 = sinf(M_PI_F*t); - float fun2 = exp(-powf((t-float(i)/_num_basis_funs),2)/sigma); - vec(i) = fun2*(M_PI_F*sigma*cosf(M_PI_F*t)-2*(t-float(i)/_num_basis_funs)*fun1)/sigma; - } - return vec; + Vector vec; + vec(0) = 0.0f; + float sigma = 1.0f / _num_basis_funs; + + for (uint i = 1; i < _num_basis_funs; i++) { + float fun1 = sinf(M_PI_F * t); + float fun2 = exp(-powf((t - float(i) / _num_basis_funs), 2) / sigma); + vec(i) = fun2 * (M_PI_F * sigma * cosf(M_PI_F * t) - 2 * (t - float(i) / _num_basis_funs) * fun1) / sigma; + } + + return vec; } Vector FixedwingPositionINDIControl::_get_d2_dt2_basis_funs(float t) { - Vector vec; - vec(0) = 0.0f; - float sigma = 1.0f/_num_basis_funs; - for(uint i=1; i<_num_basis_funs; i++){ - float fun1 = sinf(M_PI_F*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; + Vector vec; + vec(0) = 0.0f; + float sigma = 1.0f / _num_basis_funs; + + for (uint i = 1; i < _num_basis_funs; i++) { + float fun1 = sinf(M_PI_F * 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; } Vector3f FixedwingPositionINDIControl::_get_position_ref(float t) { - Vector basis = _get_basis_funs(t); - float x = _basis_coeffs_x*basis; - float y = _basis_coeffs_y*basis; - float z = _basis_coeffs_z*basis; - return _R_trajec_to_enu*Vector3f{x, y, z} + _vec_enu_to_trajec; + Vector basis = _get_basis_funs(t); + float x = _basis_coeffs_x * basis; + float y = _basis_coeffs_y * basis; + float z = _basis_coeffs_z * basis; + return _R_trajec_to_enu * Vector3f{x, y, z} + _vec_enu_to_trajec; } Vector3f FixedwingPositionINDIControl::_get_velocity_ref(float t, float T) { - Vector basis = _get_d_dt_basis_funs(t); - float x = _basis_coeffs_x*basis; - float y = _basis_coeffs_y*basis; - float z = _basis_coeffs_z*basis; - return _R_trajec_to_enu*(Vector3f{x, y, z}/T); + Vector basis = _get_d_dt_basis_funs(t); + float x = _basis_coeffs_x * basis; + float y = _basis_coeffs_y * basis; + float z = _basis_coeffs_z * basis; + return _R_trajec_to_enu * (Vector3f{x, y, z} / T); } Vector3f FixedwingPositionINDIControl::_get_acceleration_ref(float t, float T) { - Vector basis = _get_d2_dt2_basis_funs(t); - float x = _basis_coeffs_x*basis; - float y = _basis_coeffs_y*basis; - float z = _basis_coeffs_z*basis; - return _R_trajec_to_enu*(Vector3f{x, y, z}/powf(T,2)); + Vector basis = _get_d2_dt2_basis_funs(t); + float x = _basis_coeffs_x * basis; + float y = _basis_coeffs_y * basis; + float z = _basis_coeffs_z * basis; + return _R_trajec_to_enu * (Vector3f{x, y, z} / powf(T, 2)); } Quatf FixedwingPositionINDIControl::_get_attitude_ref(float t, float T) { - Vector3f vel = _get_velocity_ref(t,T); - Vector3f vel_air = vel - _wind_estimate; - Vector3f acc = _get_acceleration_ref(t,T); - // add gravity - acc(2) += 9.81f; - // compute required force - Vector3f f = _mass*acc; - // compute force component projected onto lift axis - Vector3f vel_normalized = vel_air.normalized(); - Vector3f f_lift = f - (f*vel_normalized)*vel_normalized; - Vector3f lift_normalized = f_lift.normalized(); - Vector3f wing_normalized = -vel_normalized.cross(lift_normalized); - // compute rotation matrix between ENU and FRD frame - Dcmf R_bi; - R_bi(0,0) = vel_normalized(0); - R_bi(0,1) = vel_normalized(1); - R_bi(0,2) = vel_normalized(2); - R_bi(1,0) = wing_normalized(0); - R_bi(1,1) = wing_normalized(1); - R_bi(1,2) = wing_normalized(2); - R_bi(2,0) = lift_normalized(0); - R_bi(2,1) = lift_normalized(1); - R_bi(2,2) = lift_normalized(2); - R_bi.renormalize(); - // compute actual air density to be used with true airspeed - float rho_corrected; - if (_cal_airspeed>=_stall_speed) { - rho_corrected = _rho*powf(_cal_airspeed/_true_airspeed, 2); - } - else { - rho_corrected = _rho; - } - // compute required AoA - Vector3f f_phi = R_bi*f_lift; - float AoA = ((2.f*f_phi(2))/(rho_corrected*_area*(vel_air*vel_air)+0.001f) - _C_L0)/_C_L1 - _aoa_offset; - // compute final rotation matrix - Eulerf e(0.f, AoA, 0.f); - Dcmf R_pitch(e); - Dcmf Rotation(R_pitch*R_bi); - // switch from FRD to ENU frame - Rotation(1,0) *= -1.f; - Rotation(1,1) *= -1.f; - Rotation(1,2) *= -1.f; - Rotation(2,0) *= -1.f; - Rotation(2,1) *= -1.f; - 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)) + - 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)); - */ + Vector3f vel = _get_velocity_ref(t, T); + Vector3f vel_air = vel - _wind_estimate; + Vector3f acc = _get_acceleration_ref(t, T); + // add gravity + acc(2) += 9.81f; + // compute required force + Vector3f f = _mass * acc; + // compute force component projected onto lift axis + Vector3f vel_normalized = vel_air.normalized(); + Vector3f f_lift = f - (f * vel_normalized) * vel_normalized; + Vector3f lift_normalized = f_lift.normalized(); + Vector3f wing_normalized = -vel_normalized.cross(lift_normalized); + // compute rotation matrix between ENU and FRD frame + Dcmf R_bi; + R_bi(0, 0) = vel_normalized(0); + R_bi(0, 1) = vel_normalized(1); + R_bi(0, 2) = vel_normalized(2); + R_bi(1, 0) = wing_normalized(0); + R_bi(1, 1) = wing_normalized(1); + R_bi(1, 2) = wing_normalized(2); + R_bi(2, 0) = lift_normalized(0); + R_bi(2, 1) = lift_normalized(1); + R_bi(2, 2) = lift_normalized(2); + R_bi.renormalize(); + // compute actual air density to be used with true airspeed + float rho_corrected; + + if (_cal_airspeed >= _stall_speed) { + rho_corrected = _rho * powf(_cal_airspeed / _true_airspeed, 2); + + } else { + rho_corrected = _rho; + } + + // compute required AoA + Vector3f f_phi = R_bi * f_lift; + float AoA = ((2.f * f_phi(2)) / (rho_corrected * _area * (vel_air * vel_air) + 0.001f) - _C_L0) / _C_L1 - _aoa_offset; + // compute final rotation matrix + Eulerf e(0.f, AoA, 0.f); + Dcmf R_pitch(e); + Dcmf Rotation(R_pitch * R_bi); + // switch from FRD to ENU frame + Rotation(1, 0) *= -1.f; + Rotation(1, 1) *= -1.f; + Rotation(1, 2) *= -1.f; + Rotation(2, 0) *= -1.f; + Rotation(2, 1) *= -1.f; + 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)) + + 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)); + */ - Quatf q(Rotation.transpose()); - return q; + Quatf q(Rotation.transpose()); + return q; } Vector3f FixedwingPositionINDIControl::_get_angular_velocity_ref(float t, float T) { - float dt = 0.001; - float t_lower = fmaxf(0.f,t-dt); - float t_upper = fminf(t+dt,1.f); - Dcmf R_i0(_get_attitude_ref(t_lower, T)); - Dcmf R_i1(_get_attitude_ref(t_upper, T)); - Dcmf R_10 = R_i1.transpose()*R_i0; - AxisAnglef w_01(R_10); - return -w_01.axis()*w_01.angle()/(T*(t_upper-t_lower)); + float dt = 0.001; + float t_lower = fmaxf(0.f, t - dt); + float t_upper = fminf(t + dt, 1.f); + Dcmf R_i0(_get_attitude_ref(t_lower, T)); + Dcmf R_i1(_get_attitude_ref(t_upper, T)); + Dcmf R_10 = R_i1.transpose() * R_i0; + AxisAnglef w_01(R_10); + return -w_01.axis() * w_01.angle() / (T * (t_upper - t_lower)); } Vector3f FixedwingPositionINDIControl::_get_angular_acceleration_ref(float t, float T) { - float dt = 0.001; - float t_lower = fmaxf(0.f,t-dt); - float t_upper = fminf(t+dt,1.f); - // compute roational velocity in inertial frame - Dcmf R_i0(_get_attitude_ref(t_lower, T)); - AxisAnglef w_0(R_i0*_get_angular_velocity_ref(t_lower, T)); - // compute roational velocity in inertial frame - Dcmf R_i1(_get_attitude_ref(t_upper, T)); - AxisAnglef w_1(R_i1*_get_angular_velocity_ref(t_upper, T)); - // compute gradient via finite differences - Vector3f dw_dt = (w_1.axis()*w_1.angle() - w_0.axis()*w_0.angle()) / (T*(t_upper-t_lower)); - // transform back to body frame - return R_i0.transpose()*dw_dt; + float dt = 0.001; + float t_lower = fmaxf(0.f, t - dt); + float t_upper = fminf(t + dt, 1.f); + // compute roational velocity in inertial frame + Dcmf R_i0(_get_attitude_ref(t_lower, T)); + AxisAnglef w_0(R_i0 * _get_angular_velocity_ref(t_lower, T)); + // compute roational velocity in inertial frame + Dcmf R_i1(_get_attitude_ref(t_upper, T)); + AxisAnglef w_1(R_i1 * _get_angular_velocity_ref(t_upper, T)); + // compute gradient via finite differences + Vector3f dw_dt = (w_1.axis() * w_1.angle() - w_0.axis() * w_0.angle()) / (T * (t_upper - t_lower)); + // transform back to body frame + return R_i0.transpose() * dw_dt; } float FixedwingPositionINDIControl::_get_closest_t(Vector3f pos) { - // multi-stage computation of the closest point on the reference path + // multi-stage computation of the closest point on the reference path - const uint n_1 = 20; - Vector distances; - float t_ref; - // ======= - // STAGE 1 - // ======= - // STAGE 1: compute all distances - for(uint i=0; i distances; + float t_ref; - // STAGE 1: get index of smallest distance - float t_1 = 0.f; - float min_dist = distances(0); - for(uint i=1; i distances_2; - float t_lower = fmod(t_1 - 1.0f/n_1,1.0f); - // STAGE 2: compute all distances - for(uint i=0; i<=n_2; i++){ - t_ref = fmod(t_lower + float(i)*2.f/float(n_1*n_2),1.0f); - Vector3f pos_ref = _get_position_ref(t_ref); - distances_2(i) = (pos_ref - pos)*(pos_ref - pos); - } + // STAGE 1: get index of smallest distance + float t_1 = 0.f; + float min_dist = distances(0); - // STAGE 2: get index of smallest distance - float t_2 = 0.f; - min_dist = distances_2(0); - for(uint i=1; i<=n_2; i++){ - if(distances_2(i) distances_2; + float t_lower = fmod(t_1 - 1.0f / n_1, 1.0f); + + // STAGE 2: compute all distances + for (uint i = 0; i <= n_2; i++) { + t_ref = fmod(t_lower + float(i) * 2.f / float(n_1 * n_2), 1.0f); + Vector3f pos_ref = _get_position_ref(t_ref); + distances_2(i) = (pos_ref - pos) * (pos_ref - pos); + } + + // STAGE 2: get index of smallest distance + float t_2 = 0.f; + min_dist = distances_2(0); + + for (uint i = 1; i <= n_2; i++) { + if (distances_2(i) < min_dist) { + min_dist = distances_2(i); + t_2 = float(i); + } + } + + t_2 = fmod(t_lower + float(t_2) * 2.f / (float(n_2) * float(n_1)), 1.0f); + + return t_2; } Quatf FixedwingPositionINDIControl::_get_attitude(Vector3f vel, Vector3f f) { - Vector3f vel_air = vel - _wind_estimate; - // compute force component projected onto lift axis - Vector3f vel_normalized = vel_air.normalized(); - Vector3f f_lift = f - (f*vel_normalized)*vel_normalized; - Vector3f lift_normalized = f_lift.normalized(); - Vector3f wing_normalized = -vel_normalized.cross(lift_normalized); - // compute rotation matrix between ENU and FRD frame - Dcmf R_bi; - R_bi(0,0) = vel_normalized(0); - R_bi(0,1) = vel_normalized(1); - R_bi(0,2) = vel_normalized(2); - R_bi(1,0) = wing_normalized(0); - R_bi(1,1) = wing_normalized(1); - R_bi(1,2) = wing_normalized(2); - R_bi(2,0) = lift_normalized(0); - R_bi(2,1) = lift_normalized(1); - R_bi(2,2) = lift_normalized(2); - R_bi.renormalize(); - float rho_corrected; - if (_cal_airspeed>=_stall_speed) { - rho_corrected = _rho*powf(_cal_airspeed/_true_airspeed, 2); - } - else { - rho_corrected = _rho; - } - // compute required AoA - Vector3f f_phi = R_bi*f_lift; - float AoA = ((2.f*f_phi(2))/(rho_corrected*_area*(vel_air*vel_air)+0.001f) - _C_L0)/_C_L1 - _aoa_offset; - // compute final rotation matrix - Eulerf e(0.f, AoA, 0.f); - Dcmf R_pitch(e); - Dcmf Rotation(R_pitch*R_bi); - // switch from FRD to ENU frame - Rotation(1,0) *= -1.f; - Rotation(1,1) *= -1.f; - Rotation(1,2) *= -1.f; - Rotation(2,0) *= -1.f; - Rotation(2,1) *= -1.f; - Rotation(2,2) *= -1.f; + Vector3f vel_air = vel - _wind_estimate; + // compute force component projected onto lift axis + Vector3f vel_normalized = vel_air.normalized(); + Vector3f f_lift = f - (f * vel_normalized) * vel_normalized; + Vector3f lift_normalized = f_lift.normalized(); + Vector3f wing_normalized = -vel_normalized.cross(lift_normalized); + // compute rotation matrix between ENU and FRD frame + Dcmf R_bi; + R_bi(0, 0) = vel_normalized(0); + R_bi(0, 1) = vel_normalized(1); + R_bi(0, 2) = vel_normalized(2); + R_bi(1, 0) = wing_normalized(0); + R_bi(1, 1) = wing_normalized(1); + R_bi(1, 2) = wing_normalized(2); + R_bi(2, 0) = lift_normalized(0); + R_bi(2, 1) = lift_normalized(1); + R_bi(2, 2) = lift_normalized(2); + R_bi.renormalize(); + float rho_corrected; - Quatf q(Rotation.transpose()); - return q; + if (_cal_airspeed >= _stall_speed) { + rho_corrected = _rho * powf(_cal_airspeed / _true_airspeed, 2); + + } else { + rho_corrected = _rho; + } + + // compute required AoA + Vector3f f_phi = R_bi * f_lift; + float AoA = ((2.f * f_phi(2)) / (rho_corrected * _area * (vel_air * vel_air) + 0.001f) - _C_L0) / _C_L1 - _aoa_offset; + // compute final rotation matrix + Eulerf e(0.f, AoA, 0.f); + Dcmf R_pitch(e); + Dcmf Rotation(R_pitch * R_bi); + // switch from FRD to ENU frame + Rotation(1, 0) *= -1.f; + Rotation(1, 1) *= -1.f; + Rotation(1, 2) *= -1.f; + Rotation(2, 0) *= -1.f; + Rotation(2, 1) *= -1.f; + Rotation(2, 2) *= -1.f; + + Quatf q(Rotation.transpose()); + return q; } Vector3f -FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f vel_ref, Vector3f acc_ref, Vector3f omega_ref, Vector3f alpha_ref) +FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f vel_ref, Vector3f acc_ref, + Vector3f omega_ref, Vector3f alpha_ref) { - Dcmf R_ib(_att); - Dcmf R_bi(R_ib.transpose()); - // apply LP filter to acceleration & velocity - Vector3f acc_filtered; - acc_filtered(0) = _lp_filter_accel[0].apply(_acc(0)); - acc_filtered(1) = _lp_filter_accel[1].apply(_acc(1)); - acc_filtered(2) = _lp_filter_accel[2].apply(_acc(2)); - Vector3f omega_filtered; - omega_filtered(0) = _lp_filter_omega[0].apply(_omega(0)); - omega_filtered(1) = _lp_filter_omega[1].apply(_omega(1)); - omega_filtered(2) = _lp_filter_omega[2].apply(_omega(2)); + Dcmf R_ib(_att); + Dcmf R_bi(R_ib.transpose()); + // apply LP filter to acceleration & velocity + Vector3f acc_filtered; + acc_filtered(0) = _lp_filter_accel[0].apply(_acc(0)); + acc_filtered(1) = _lp_filter_accel[1].apply(_acc(1)); + acc_filtered(2) = _lp_filter_accel[2].apply(_acc(2)); + Vector3f omega_filtered; + omega_filtered(0) = _lp_filter_omega[0].apply(_omega(0)); + omega_filtered(1) = _lp_filter_omega[1].apply(_omega(1)); + omega_filtered(2) = _lp_filter_omega[2].apply(_omega(2)); - // ========================================= - // apply PD control law on the body position - // ========================================= - Vector3f acc_command = R_ib*(_K_x*R_bi*(pos_ref-_pos) + _K_v*R_bi*(vel_ref-_vel) + _K_a*R_bi*(acc_ref-_acc)) + acc_ref; + // ========================================= + // apply PD control law on the body position + // ========================================= + Vector3f acc_command = R_ib * (_K_x * R_bi * (pos_ref - _pos) + _K_v * R_bi * (vel_ref - _vel) + _K_a * R_bi * + (acc_ref - _acc)) + acc_ref; - // ================================== - // compute expected aerodynamic force - // ================================== - Vector3f f_current; - 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); - // compute actual air density - float rho_corrected; - if (_cal_airspeed>=_stall_speed) { - rho_corrected = _rho*powf(_cal_airspeed/_true_airspeed, 2); - } - else { - rho_corrected = _rho; - } - float factor = -0.5f*rho_corrected*_area*sqrtf(vel_body*vel_body); - Vector3f w_x = vel_body; - Vector3f w_z = w_x.cross(Vector3f{0.f,1.f,0.f}); - f_current = R_ib*(factor*(C_l*w_z + C_d*w_x)); - // apply LP filter to force - Vector3f f_current_filtered; - f_current_filtered(0) = _lp_filter_force[0].apply(f_current(0)); - f_current_filtered(1) = _lp_filter_force[1].apply(f_current(1)); - f_current_filtered(2) = _lp_filter_force[2].apply(f_current(2)); + // ================================== + // compute expected aerodynamic force + // ================================== + Vector3f f_current; + 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); + // compute actual air density + float rho_corrected; - // ================================ - // get force command in world frame - // ================================ - Vector3f f_command = _mass*(acc_command - acc_filtered) + f_current_filtered; + if (_cal_airspeed >= _stall_speed) { + rho_corrected = _rho * powf(_cal_airspeed / _true_airspeed, 2); - // ============================================================================================================ - // apply some filtering to the force command. This introduces some time delay, - // which is not desired for stability reasons, but it rejects some of the noise fed to the low-level controller - // ============================================================================================================ - f_command(0) = _lp_filter_ctrl0[0].apply(f_command(0)); - f_command(1) = _lp_filter_ctrl0[1].apply(f_command(1)); - f_command(2) = _lp_filter_ctrl0[2].apply(f_command(2)); - _f_command = f_command; - // limit maximum lift force by the maximum lift force, the aircraft can produce (assume max force at 15° aoa) + } else { + rho_corrected = _rho; + } - // ==================================================================== - // saturate force command to avoid overly agressive maneuvers and stall - // ==================================================================== - if (_switch_saturation){ - float speed = vel_body*vel_body; - // compute maximum achievable force - float f_max; - if (speed>_stall_speed){ - f_max = -factor*sqrtf(vel_body*vel_body)*(_C_L0 + _C_L1*0.25f); // assume stall at 15° AoA - } - else { - f_max = -factor*_stall_speed*(_C_L0 + _C_L1*0.25f); // assume stall at 15° AoA - } - // compute current command - float f_now = sqrtf(f_command*f_command); - // saturate current command - if (f_now>f_max){ - f_command = f_max/f_now * f_command; - } - } + float factor = -0.5f * rho_corrected * _area * sqrtf(vel_body * vel_body); + Vector3f w_x = vel_body; + Vector3f w_z = w_x.cross(Vector3f{0.f, 1.f, 0.f}); + f_current = R_ib * (factor * (C_l * w_z + C_d * w_x)); + // apply LP filter to force + Vector3f f_current_filtered; + f_current_filtered(0) = _lp_filter_force[0].apply(f_current(0)); + f_current_filtered(1) = _lp_filter_force[1].apply(f_current(1)); + f_current_filtered(2) = _lp_filter_force[2].apply(f_current(2)); - // ========================================================================== - // get required attitude (assuming we can fly the target velocity), and error - // ========================================================================== - Dcmf R_ref(_get_attitude(vel_ref,f_command)); - // get attitude error - Dcmf R_ref_true(R_ref.transpose()*R_ib); - // get required rotation vector (in body frame) - AxisAnglef q_err(R_ref_true); - Vector3f w_err; - // project rotation angle to [-pi,pi] - if (q_err.angle()*q_err.angle()0.f){ - w_err = (2.f*M_PI_F-(float)fmod(q_err.angle(),2.f*M_PI_F))*q_err.axis(); - } - else{ - w_err = (-2.f*M_PI_F-(float)fmod(q_err.angle(),2.f*M_PI_F))*q_err.axis(); - } - } + // ================================ + // get force command in world frame + // ================================ + Vector3f f_command = _mass * (acc_command - acc_filtered) + f_current_filtered; - // ========================================= - // apply PD control law on the body attitude - // ========================================= - Vector3f rot_acc_command = _K_q*w_err + _K_w*(omega_ref-_omega) + alpha_ref; + // ============================================================================================================ + // apply some filtering to the force command. This introduces some time delay, + // which is not desired for stability reasons, but it rejects some of the noise fed to the low-level controller + // ============================================================================================================ + f_command(0) = _lp_filter_ctrl0[0].apply(f_command(0)); + f_command(1) = _lp_filter_ctrl0[1].apply(f_command(1)); + f_command(2) = _lp_filter_ctrl0[2].apply(f_command(2)); + _f_command = f_command; + // limit maximum lift force by the maximum lift force, the aircraft can produce (assume max force at 15° aoa) - if (sqrtf(w_err*w_err)>M_PI_F){ - PX4_ERR("rotation angle larger than pi: \t%.2f, \t%.2f, \t%.2f", (double)sqrtf(w_err*w_err), (double)q_err.angle(), (double)(q_err.axis()*q_err.axis())); - } + // ==================================================================== + // saturate force command to avoid overly agressive maneuvers and stall + // ==================================================================== + if (_switch_saturation) { + float speed = vel_body * vel_body; + // compute maximum achievable force + float f_max; - // ==================================== - // manual attitude setpoint feedthrough - // ==================================== - 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; - Eulerf E_current(Quatf(_attitude.q)); - float yaw_ref = E_current.psi(); - Dcmf R_ned_frd_ref(Eulerf(roll_ref, pitch_ref, yaw_ref)); - Dcmf R_enu_frd_ref(_R_ned_to_enu*R_ned_frd_ref); - Quatf att_ref(R_enu_frd_ref); - R_ref = Dcmf(att_ref); + if (speed > _stall_speed) { + f_max = -factor * sqrtf(vel_body * vel_body) * (_C_L0 + _C_L1 * 0.25f); // assume stall at 15° AoA - // get attitude error - R_ref_true = Dcmf(R_ref.transpose()*R_ib); - // get required rotation vector (in body frame) - q_err = AxisAnglef(R_ref_true); - // project rotation angle to [-pi,pi] - if (q_err.angle()*q_err.angle()0.f){ - w_err = (2.f*M_PI_F-(float)fmod(q_err.angle(),2.f*M_PI_F))*q_err.axis(); - } - else{ - w_err = (-2.f*M_PI_F-(float)fmod(q_err.angle(),2.f*M_PI_F))*q_err.axis(); - } - } - _w_err = w_err; + } else { + f_max = -factor * _stall_speed * (_C_L0 + _C_L1 * 0.25f); // assume stall at 15° AoA + } - // compute rot acc command - rot_acc_command = _K_q*w_err + _K_w*(Vector3f{0.f,0.f,0.f}-_omega); - - } + // compute current command + float f_now = sqrtf(f_command * f_command); - // ============================================================== - // overwrite rudder rot_acc_command with turn coordination values - // ============================================================== - Vector3f vel_air = _vel - _wind_estimate; - Vector3f vel_normalized = vel_air.normalized(); - Vector3f acc_normalized = acc_filtered.normalized(); - // compute ideal angular velocity - Vector3f omega_turn_ref_normalized = vel_normalized.cross(acc_normalized); - Vector3f omega_turn_ref; - // constuct acc perpendicular to flight path - Vector3f acc_perp = acc_filtered - (acc_filtered*vel_normalized)*vel_normalized; - if (_airspeed_valid&&_cal_airspeed>_stall_speed) { - omega_turn_ref = sqrtf(acc_perp*acc_perp) / (_true_airspeed) * R_bi * omega_turn_ref_normalized.normalized(); - //PX4_INFO("yaw rate ref, yaw rate: \t%.2f\t%.2f", (double)(omega_turn_ref(2)), (double)(omega_filtered(2))); - } - else { - omega_turn_ref = sqrtf(acc_perp*acc_perp) / (_stall_speed) * R_bi * omega_turn_ref_normalized.normalized(); - //PX4_ERR("No valid airspeed message detected or airspeed too low"); - } + // saturate current command + if (f_now > f_max) { + f_command = f_max / f_now * f_command; + } + } - // 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; + // ========================================================================== + // get required attitude (assuming we can fly the target velocity), and error + // ========================================================================== + Dcmf R_ref(_get_attitude(vel_ref, f_command)); + // get attitude error + Dcmf R_ref_true(R_ref.transpose()*R_ib); + // get required rotation vector (in body frame) + AxisAnglef q_err(R_ref_true); + Vector3f w_err; - - return rot_acc_command; + // project rotation angle to [-pi,pi] + if (q_err.angle()*q_err.angle() < M_PI_F * M_PI_F) { + w_err = -q_err.angle() * q_err.axis(); + + } else { + if (q_err.angle() > 0.f) { + w_err = (2.f * M_PI_F - (float)fmod(q_err.angle(), 2.f * M_PI_F)) * q_err.axis(); + + } else { + w_err = (-2.f * M_PI_F - (float)fmod(q_err.angle(), 2.f * M_PI_F)) * q_err.axis(); + } + } + + // ========================================= + // apply PD control law on the body attitude + // ========================================= + Vector3f rot_acc_command = _K_q * w_err + _K_w * (omega_ref - _omega) + alpha_ref; + + if (sqrtf(w_err * w_err) > M_PI_F) { + PX4_ERR("rotation angle larger than pi: \t%.2f, \t%.2f, \t%.2f", (double)sqrtf(w_err * w_err), (double)q_err.angle(), + (double)(q_err.axis()*q_err.axis())); + } + + // ==================================== + // manual attitude setpoint feedthrough + // ==================================== + 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; + Eulerf E_current(Quatf(_attitude.q)); + float yaw_ref = E_current.psi(); + Dcmf R_ned_frd_ref(Eulerf(roll_ref, pitch_ref, yaw_ref)); + Dcmf R_enu_frd_ref(_R_ned_to_enu * R_ned_frd_ref); + Quatf att_ref(R_enu_frd_ref); + R_ref = Dcmf(att_ref); + + // get attitude error + R_ref_true = Dcmf(R_ref.transpose() * R_ib); + // get required rotation vector (in body frame) + q_err = AxisAnglef(R_ref_true); + + // project rotation angle to [-pi,pi] + if (q_err.angle()*q_err.angle() < M_PI_F * M_PI_F) { + w_err = -q_err.angle() * q_err.axis(); + + } else { + if (q_err.angle() > 0.f) { + w_err = (2.f * M_PI_F - (float)fmod(q_err.angle(), 2.f * M_PI_F)) * q_err.axis(); + + } else { + w_err = (-2.f * M_PI_F - (float)fmod(q_err.angle(), 2.f * M_PI_F)) * q_err.axis(); + } + } + + _w_err = w_err; + + // compute rot acc command + rot_acc_command = _K_q * w_err + _K_w * (Vector3f{0.f, 0.f, 0.f} -_omega); + + } + + // ============================================================== + // overwrite rudder rot_acc_command with turn coordination values + // ============================================================== + Vector3f vel_air = _vel - _wind_estimate; + Vector3f vel_normalized = vel_air.normalized(); + Vector3f acc_normalized = acc_filtered.normalized(); + // compute ideal angular velocity + Vector3f omega_turn_ref_normalized = vel_normalized.cross(acc_normalized); + Vector3f omega_turn_ref; + // constuct acc perpendicular to flight path + Vector3f acc_perp = acc_filtered - (acc_filtered * vel_normalized) * vel_normalized; + + if (_airspeed_valid && _cal_airspeed > _stall_speed) { + omega_turn_ref = sqrtf(acc_perp * acc_perp) / (_true_airspeed) * R_bi * omega_turn_ref_normalized.normalized(); + //PX4_INFO("yaw rate ref, yaw rate: \t%.2f\t%.2f", (double)(omega_turn_ref(2)), (double)(omega_filtered(2))); + + } else { + omega_turn_ref = sqrtf(acc_perp * acc_perp) / (_stall_speed) * R_bi * omega_turn_ref_normalized.normalized(); + //PX4_ERR("No valid airspeed message detected or airspeed too low"); + } + + // 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 - Dcmf R_ib(_att); - Vector3f vel_body = R_ib.transpose()*(_vel-_wind_estimate); - float q = fmaxf(0.5f*sqrtf(vel_body*vel_body)*vel_body(0), 0.5f*_stall_speed*_stall_speed); // dynamic pressure, saturates at stall speed + // compute velocity in body frame + Dcmf R_ib(_att); + Vector3f vel_body = R_ib.transpose() * (_vel - _wind_estimate); + float q = fmaxf(0.5f * sqrtf(vel_body * vel_body) * vel_body(0), + 0.5f * _stall_speed * _stall_speed); // dynamic pressure, saturates at stall speed - // 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(2) = 0.f; + // 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(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)); + // 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)); - // No filter for alpha, since it is already filtered... - Vector3f alpha_filtered = _alpha; - Vector3f moment_command = _inertia * (ctrl - alpha_filtered) + moment_filtered; - _m_command = R_ib.transpose()*moment_command; + // No filter for alpha, since it is already filtered... + Vector3f alpha_filtered = _alpha; + Vector3f moment_command = _inertia * (ctrl - alpha_filtered) + moment_filtered; + _m_command = R_ib.transpose() * moment_command; - // perform dynamic inversion - Vector3f deflection; - deflection(0) = (moment_command(0) + _k_d_roll*q*_omega(0))/fmaxf((_k_ail*q),0.0001f); - deflection(1) = (moment_command(1) + _k_d_pitch*q*_omega(1))/fmaxf((_k_ele*q),0.0001f); + // perform dynamic inversion + Vector3f deflection; + deflection(0) = (moment_command(0) + _k_d_roll * q * _omega(0)) / fmaxf((_k_ail * q), 0.0001f); + deflection(1) = (moment_command(1) + _k_d_pitch * q * _omega(1)) / fmaxf((_k_ele * q), 0.0001f); - // overwrite rudder deflection with NDI turn coordination (no INDI) - deflection(2) = ctrl(2); + // overwrite rudder deflection with NDI turn coordination (no INDI) + deflection(2) = ctrl(2); - return deflection; + return deflection; } Vector3f FixedwingPositionINDIControl::_compute_actuator_deflections(Vector3f ctrl) -{ - // compute the normalized actuator deflection, including airspeed scaling - Vector3f deflection = ctrl; +{ + // compute the normalized actuator deflection, including airspeed scaling + Vector3f deflection = ctrl; - // limit actuator deflection - for(int i=0; i<3; i++){ - deflection(i) = constrain(deflection(i),-1.f,1.f); - } - /* - // add servo slew - float current_ail = _actuators.control[actuator_controls_s::INDEX_ROLL]; - float current_ele = _actuators.control[actuator_controls_s::INDEX_PITCH]; - float current_rud = _actuators.control[actuator_controls_s::INDEX_YAW]; - // - float max_rate = 0.5f/0.18f; // - float dt = 1.f/_sample_frequency; - // - deflection(0) = constrain(deflection(0),current_ail-dt*max_rate,current_ail+dt*max_rate); - deflection(1) = constrain(deflection(1),current_ele-dt*max_rate,current_ele+dt*max_rate); - deflection(2) = constrain(deflection(2),current_rud-dt*max_rate,current_rud+dt*max_rate); - */ - return deflection; + // limit actuator deflection + for (int i = 0; i < 3; i++) { + deflection(i) = constrain(deflection(i), -1.f, 1.f); + } + + /* + // add servo slew + float current_ail = _actuators.control[actuator_controls_s::INDEX_ROLL]; + float current_ele = _actuators.control[actuator_controls_s::INDEX_PITCH]; + float current_rud = _actuators.control[actuator_controls_s::INDEX_YAW]; + // + float max_rate = 0.5f/0.18f; // + float dt = 1.f/_sample_frequency; + // + deflection(0) = constrain(deflection(0),current_ail-dt*max_rate,current_ail+dt*max_rate); + deflection(1) = constrain(deflection(1),current_ele-dt*max_rate,current_ele+dt*max_rate); + deflection(2) = constrain(deflection(2),current_rud-dt*max_rate,current_rud+dt*max_rate); + */ + return deflection; } diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp index 021b45870d..014d53b413 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp @@ -1,5 +1,5 @@ /** - * Implementation of a generic incremental position controller + * Implementation of a generic incremental position controller * using incremental nonlinear dynamic inversion and differential flatness * for a fixed wing aircraft during dynamic soaring cycles. * The controller directly outputs actuator deflections for ailerons, elevator and rudder. @@ -115,23 +115,23 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - // Subscriptions + // 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 _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 _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_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 _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)}; - - // Publishers + + // Publishers uORB::Publication _actuators_0_pub; uORB::Publication _attitude_sp_pub; uORB::Publication _angular_vel_sp_pub{ORB_ID(vehicle_rates_setpoint)}; @@ -144,7 +144,7 @@ private: uORB::Publication _offboard_control_mode_pub{ORB_ID(offboard_control_mode)}; uORB::Publication _debug_value_pub{ORB_ID(debug_value)}; - // Message structs + // Message structs vehicle_angular_acceleration_setpoint_s _angular_accel_sp {}; actuator_controls_s _actuators {}; // actuator commands manual_control_setpoint_s _manual_control_setpoint {}; ///< r/c channel data @@ -164,7 +164,7 @@ private: 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_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 @@ -245,7 +245,7 @@ private: uint8_t _pos_reset_counter{0}; ///< captures the number of times the estimator has reset the horizontal position uint8_t _alt_reset_counter{0}; ///< captures the number of times the estimator has reset the altitude state - + // Update our local parameter cache. int parameters_update(); @@ -261,7 +261,7 @@ private: void vehicle_angular_velocity_poll(); void vehicle_angular_acceleration_poll(); void actuator_controls_poll(); - + void control_update(); void manual_control_setpoint_poll(); void rc_channels_poll(); @@ -281,30 +281,42 @@ private: void _select_loiter_trajectory(); // select the correct loiter trajectory based on available energy void _select_soaring_trajectory(); // select the correct loiter trajectory based on available energy void _read_trajectory_coeffs_csv(char *filename); // read in the correct coefficients of the appropriate trajectory - float _get_closest_t(Vector3f pos); // get the normalized time, at which the reference path is closest to the current position + float _get_closest_t(Vector3f + pos); // get the normalized time, at which the reference path is closest to the current position Vector3f _compute_wind_estimate(); // compute a wind estimate to be used only inside the controller Vector3f _compute_wind_estimate_EKF(); // compute a wind estimate to be used only as a measurement for the shear estimator void _set_wind_estimate(Vector3f wind); void _set_wind_estimate_EKF(Vector3f wind); - Vector _get_basis_funs(float t=0); // compute the vector of basis functions at normalized time t in [0,1] - Vector _get_d_dt_basis_funs(float t=0); // compute the vector of basis function gradients at normalized time t in [0,1] - Vector _get_d2_dt2_basis_funs(float t=0); // compute the vector of basis function curvatures at normalized time t in [0,1] + Vector _get_basis_funs(float t = + 0); // compute the vector of basis functions at normalized time t in [0,1] + Vector _get_d_dt_basis_funs(float t = + 0); // compute the vector of basis function gradients at normalized time t in [0,1] + Vector _get_d2_dt2_basis_funs(float t = + 0); // compute the vector of basis function curvatures at normalized time t in [0,1] void _load_basis_coefficients(); // load the coefficients of the current path approximation - Vector3f _get_position_ref(float t=0); // get the reference position on the current path, at normalized time t in [0,1] - Vector3f _get_velocity_ref(float t=0, float T=1); // get the reference velocity on the current path, at normalized time t in [0,1], with an intended cycle time of T - Vector3f _get_acceleration_ref(float t=0, float T=1); // get the reference acceleration on the current path, at normalized time t in [0,1], with an intended cycle time of T - Quatf _get_attitude_ref(float t=0, float T=1); // get the reference attitude on the current path, at normalized time t in [0,1], with an intended cycle time of T - Vector3f _get_angular_velocity_ref(float t=0, float T=1); // get the reference angular velocity on the current path, at normalized time t in [0,1], with an intended cycle time of T - Vector3f _get_angular_acceleration_ref(float t=0, float T=1); // get the reference angular acceleration on the current path, at normalized time t in [0,1], with an intended cycle time of T - Quatf _get_attitude(Vector3f vel, Vector3f f); // get the attitude to produce force f while flying with velocity vel - Vector3f _compute_INDI_stage_1(Vector3f pos_ref, Vector3f vel_ref, Vector3f acc_ref, Vector3f omega_ref, Vector3f alpha_ref); + Vector3f _get_position_ref(float t = + 0); // get the reference position on the current path, at normalized time t in [0,1] + Vector3f _get_velocity_ref(float t = 0, + float T = 1); // get the reference velocity on the current path, at normalized time t in [0,1], with an intended cycle time of T + Vector3f _get_acceleration_ref(float t = 0, + float T = 1); // get the reference acceleration on the current path, at normalized time t in [0,1], with an intended cycle time of T + Quatf _get_attitude_ref(float t = 0, + float T = 1); // get the reference attitude on the current path, at normalized time t in [0,1], with an intended cycle time of T + Vector3f _get_angular_velocity_ref(float t = 0, + float T = 1); // get the reference angular velocity on the current path, at normalized time t in [0,1], with an intended cycle time of T + Vector3f _get_angular_acceleration_ref(float t = 0, + float T = 1); // get the reference angular acceleration on the current path, at normalized time t in [0,1], with an intended cycle time of T + Quatf _get_attitude(Vector3f vel, Vector3f + f); // get the attitude to produce force f while flying with velocity vel + Vector3f _compute_INDI_stage_1(Vector3f pos_ref, Vector3f vel_ref, Vector3f acc_ref, Vector3f omega_ref, + Vector3f alpha_ref); Vector3f _compute_INDI_stage_2(Vector3f ctrl); Vector3f _compute_actuator_deflections(Vector3f ctrl); // helper methods - void _reverse(char* str, int len); // reverse a string of length 'len' + void _reverse(char *str, int len); // reverse a string of length 'len' int _int_to_str(int x, char str[], int d); // convert an integer x into a string of length d - void _float_to_str(float n, char* res, int afterpoint); // convert float to string + void _float_to_str(float n, char *res, int afterpoint); // convert float to string // control variables @@ -326,9 +338,9 @@ private: Vector3f _omega; // angular rate vector Vector3f _alpha; // angular acceleration vector float _k_ail; - float _k_ele; - float _k_d_roll; - float _k_d_pitch; + float _k_ele; + float _k_d_roll; + float _k_d_pitch; hrt_abstime _last_run{0}; // controller frequency @@ -339,9 +351,10 @@ private: 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) + 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_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 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 9feda06893..ba4073b26a 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,8 +554,8 @@ 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 @@ -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,7 +617,7 @@ 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 @@ -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 diff --git a/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp b/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp index 1700345f74..5736769b00 100644 --- a/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp +++ b/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp @@ -48,7 +48,7 @@ using matrix::geninv; FixedwingShearEstimator::FixedwingShearEstimator() : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::test1), - _soaring_estimator_shear_pub(ORB_ID(soaring_estimator_shear)), + _soaring_estimator_shear_pub(ORB_ID(soaring_estimator_shear)), _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) { // limit to 10 Hz @@ -70,124 +70,132 @@ FixedwingShearEstimator::init() PX4_ERR("vehicle position callback registration failed!"); return false; } - PX4_INFO("Starting FW_DYN_SOAR_ESTIMATOR"); - // normalization variables - _unit_v = 1.f; - _unit_h = 1.f; - _unit_a = 1.f; + PX4_INFO("Starting FW_DYN_SOAR_ESTIMATOR"); - parameters_update(); + // normalization variables + _unit_v = 1.f; + _unit_h = 1.f; + _unit_a = 1.f; - // init time - _last_run = hrt_absolute_time(); + parameters_update(); - // init reset counter - _reset_counter = -1; - reset_filter(); + // init time + _last_run = hrt_absolute_time(); - // - _v_max_lock = 8.f; - _alpha_lock = 0.5f; + // init reset counter + _reset_counter = -1; + reset_filter(); - // ============================================================================== - // fill the min airspeed matrix with the correct entries for trajectory selection - // ============================================================================== - - 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/"; - strcat(home_dir, "robust/min_aspd_matrix.csv"); - FILE* fp = fopen(home_dir, "r"); + // + _v_max_lock = 8.f; + _alpha_lock = 0.5f; - if (fp == nullptr) { - PX4_ERR("Can't open MIN_ASPD_MATRIX"); - error = true; - } - else { - // Here we have taken size of - // array 200 you can modify it - const uint buffersize = 600; - char buffer[buffersize]; + // ============================================================================== + // fill the min airspeed matrix with the correct entries for trajectory selection + // ============================================================================== - int row = 0; - int column = 0; - // loop over rows - while (fgets(buffer, - buffersize, fp)) { - column = 0; - - // Splitting the data - char* value = strtok(buffer, ","); - - // loop over columns - while (value) { - if (*value=='\0'||*value==' ') { - // simply skip extra characters - continue; - } - if (row<(int)_num_v_max && column<(int)_num_alpha) { - _MIN_ASPD_MATRIX(row,column) = (float)atof(value); - _MAX_ASPD_MATRIX(row,column) = (float)atof(value) + 4.f; - } - else { - PX4_ERR("Encountered invalid MIN_ASPD_MATRIX"); - } - //PX4_INFO("row: %d, col: %d, read value: %.3f", row, column, (double)atof(value)); - value = strtok(NULL, ","); - column++; - - } - row++; - } - int failure = fclose(fp); - if (failure==-1) { - PX4_ERR("Can't close MIN_ASPD_MATRIX"); - } - } - // ======================================================================= + bool error = false; - if (error) { - /* - // column 0: wind speed = 8 - _MIN_ASPD_MATRIX(0,0) = 28; - _MIN_ASPD_MATRIX(1,0) = 24; - _MIN_ASPD_MATRIX(2,0) = 22; - _MIN_ASPD_MATRIX(3,0) = 20; - _MIN_ASPD_MATRIX(4,0) = 20; - _MIN_ASPD_MATRIX(5,0) = 20; - _MIN_ASPD_MATRIX(6,0) = 20; - _MIN_ASPD_MATRIX(7,0) = 20; - _MIN_ASPD_MATRIX(8,0) = 20; + //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/"; + strcat(home_dir, "robust/min_aspd_matrix.csv"); + FILE *fp = fopen(home_dir, "r"); - // column 0: wind speed = 9 - _MIN_ASPD_MATRIX(0,0) = 26; - _MIN_ASPD_MATRIX(1,0) = 22; - _MIN_ASPD_MATRIX(2,0) = 20; - _MIN_ASPD_MATRIX(3,0) = 20; - _MIN_ASPD_MATRIX(4,0) = 20; - _MIN_ASPD_MATRIX(5,0) = 20; - _MIN_ASPD_MATRIX(6,0) = 20; - _MIN_ASPD_MATRIX(7,0) = 18; - _MIN_ASPD_MATRIX(8,0) = 18; + if (fp == nullptr) { + PX4_ERR("Can't open MIN_ASPD_MATRIX"); + error = true; - // column 0: wind speed = 10 - _MIN_ASPD_MATRIX(0,0) = 24; - _MIN_ASPD_MATRIX(1,0) = 22; - _MIN_ASPD_MATRIX(2,0) = 20; - _MIN_ASPD_MATRIX(3,0) = 20; - _MIN_ASPD_MATRIX(4,0) = 20; - _MIN_ASPD_MATRIX(5,0) = 20; - _MIN_ASPD_MATRIX(6,0) = 18; - _MIN_ASPD_MATRIX(7,0) = 18; - _MIN_ASPD_MATRIX(8,0) = 18; - */ + } else { + // Here we have taken size of + // array 200 you can modify it + const uint buffersize = 600; + char buffer[buffersize]; - } + int row = 0; + int column = 0; + + // loop over rows + while (fgets(buffer, + buffersize, fp)) { + column = 0; + + // Splitting the data + char *value = strtok(buffer, ","); + + // loop over columns + while (value) { + if (*value == '\0' || *value == ' ') { + // simply skip extra characters + continue; + } + + if (row < (int)_num_v_max && column < (int)_num_alpha) { + _MIN_ASPD_MATRIX(row, column) = (float)atof(value); + _MAX_ASPD_MATRIX(row, column) = (float)atof(value) + 4.f; + + } else { + PX4_ERR("Encountered invalid MIN_ASPD_MATRIX"); + } + + //PX4_INFO("row: %d, col: %d, read value: %.3f", row, column, (double)atof(value)); + value = strtok(NULL, ","); + column++; + + } + + row++; + } + + int failure = fclose(fp); + + if (failure == -1) { + PX4_ERR("Can't close MIN_ASPD_MATRIX"); + } + } + + // ======================================================================= + + if (error) { + /* + // column 0: wind speed = 8 + _MIN_ASPD_MATRIX(0,0) = 28; + _MIN_ASPD_MATRIX(1,0) = 24; + _MIN_ASPD_MATRIX(2,0) = 22; + _MIN_ASPD_MATRIX(3,0) = 20; + _MIN_ASPD_MATRIX(4,0) = 20; + _MIN_ASPD_MATRIX(5,0) = 20; + _MIN_ASPD_MATRIX(6,0) = 20; + _MIN_ASPD_MATRIX(7,0) = 20; + _MIN_ASPD_MATRIX(8,0) = 20; + + // column 0: wind speed = 9 + _MIN_ASPD_MATRIX(0,0) = 26; + _MIN_ASPD_MATRIX(1,0) = 22; + _MIN_ASPD_MATRIX(2,0) = 20; + _MIN_ASPD_MATRIX(3,0) = 20; + _MIN_ASPD_MATRIX(4,0) = 20; + _MIN_ASPD_MATRIX(5,0) = 20; + _MIN_ASPD_MATRIX(6,0) = 20; + _MIN_ASPD_MATRIX(7,0) = 18; + _MIN_ASPD_MATRIX(8,0) = 18; + + // column 0: wind speed = 10 + _MIN_ASPD_MATRIX(0,0) = 24; + _MIN_ASPD_MATRIX(1,0) = 22; + _MIN_ASPD_MATRIX(2,0) = 20; + _MIN_ASPD_MATRIX(3,0) = 20; + _MIN_ASPD_MATRIX(4,0) = 20; + _MIN_ASPD_MATRIX(5,0) = 20; + _MIN_ASPD_MATRIX(6,0) = 18; + _MIN_ASPD_MATRIX(7,0) = 18; + _MIN_ASPD_MATRIX(8,0) = 18; + */ + + } - return true; + return true; } int @@ -196,25 +204,25 @@ FixedwingShearEstimator::parameters_update() updateParams(); // update params... - _Q_horizontal.setZero(); - _Q_horizontal(0,0) = powf(_param_sigma_q_vel.get()/_unit_v,2); - _Q_horizontal(1,1) = powf(_param_sigma_q_vel.get()/_unit_v,2); - _Q_horizontal(2,2) = powf(_param_sigma_q_vel.get()/_unit_v,2); - _Q_horizontal(3,3) = powf(_param_sigma_q_vel.get()/_unit_v,2); - _Q_horizontal(4,4) = powf(_param_sigma_q_h.get()/_unit_h,2); - _Q_horizontal(5,5) = powf(_param_sigma_q_a.get()/_unit_a,2); + _Q_horizontal.setZero(); + _Q_horizontal(0, 0) = powf(_param_sigma_q_vel.get() / _unit_v, 2); + _Q_horizontal(1, 1) = powf(_param_sigma_q_vel.get() / _unit_v, 2); + _Q_horizontal(2, 2) = powf(_param_sigma_q_vel.get() / _unit_v, 2); + _Q_horizontal(3, 3) = powf(_param_sigma_q_vel.get() / _unit_v, 2); + _Q_horizontal(4, 4) = powf(_param_sigma_q_h.get() / _unit_h, 2); + _Q_horizontal(5, 5) = powf(_param_sigma_q_a.get() / _unit_a, 2); - _R_horizontal.setZero(); - _R_horizontal(0,0) = powf(_param_sigma_r_vel.get()/_unit_v,2); - _R_horizontal(1,1) = powf(_param_sigma_r_vel.get()/_unit_v,2); + _R_horizontal.setZero(); + _R_horizontal(0, 0) = powf(_param_sigma_r_vel.get() / _unit_v, 2); + _R_horizontal(1, 1) = powf(_param_sigma_r_vel.get() / _unit_v, 2); - for (int i=0;i<(int)_dim_vertical;i++){ - _Q_vertical(i,i) = powf(_param_sigma_q_vel.get(),2); - } + for (int i = 0; i < (int)_dim_vertical; i++) { + _Q_vertical(i, i) = powf(_param_sigma_q_vel.get(), 2); + } - _R_vertical(0,0) = powf(_param_sigma_r_vel.get(),2); + _R_vertical(0, 0) = powf(_param_sigma_r_vel.get(), 2); - _init_height = _param_init_h.get()/_unit_h; + _init_height = _param_init_h.get() / _unit_h; return PX4_OK; } @@ -222,143 +230,151 @@ FixedwingShearEstimator::parameters_update() void FixedwingShearEstimator::reset_filter() { - // reset all states of the filter to some initial guess. + // reset all states of the filter to some initial guess. - // reset horizontal wind state - for (uint i=0;i<6;i++){ - _X_prior_horizontal(i) = 0.0f; - _X_posterior_horizontal(i) = 0.0f; - } - _P_prior_horizontal = 10.f*_Q_horizontal; - _P_posterior_horizontal = 10.f*_Q_horizontal; + // reset horizontal wind state + for (uint i = 0; i < 6; i++) { + _X_prior_horizontal(i) = 0.0f; + _X_posterior_horizontal(i) = 0.0f; + } - // reset vertical wind state - for (uint i=0;i<_dim_vertical;i++){ - _X_prior_vertical(i) = 0.0f; - _X_posterior_vertical(i) = 0.0f; - } - _P_prior_vertical = 10.f*_Q_vertical; - _P_posterior_vertical = 10.f*_Q_vertical; + _P_prior_horizontal = 10.f * _Q_horizontal; + _P_posterior_horizontal = 10.f * _Q_horizontal; - // set height to enable convergence - _X_prior_horizontal(4) = _init_height; - _X_posterior_horizontal(4) = _init_height; - _X_prior_horizontal(5) = 0.5f/_unit_a; - _X_posterior_horizontal(5) = 0.5f/_unit_a; + // reset vertical wind state + for (uint i = 0; i < _dim_vertical; i++) { + _X_prior_vertical(i) = 0.0f; + _X_posterior_vertical(i) = 0.0f; + } - // increment counter - _reset_counter += 1; + _P_prior_vertical = 10.f * _Q_vertical; + _P_posterior_vertical = 10.f * _Q_vertical; + + // set height to enable convergence + _X_prior_horizontal(4) = _init_height; + _X_posterior_horizontal(4) = _init_height; + _X_prior_horizontal(5) = 0.5f / _unit_a; + _X_posterior_horizontal(5) = 0.5f / _unit_a; + + // increment counter + _reset_counter += 1; } void FixedwingShearEstimator::perform_prior_update() { - // get time since last run - float dt = (float)(hrt_absolute_time() - _last_run)/1000000.f; - _last_run = hrt_absolute_time(); - if (dt>1.f) { - dt = 1.f; - } + // get time since last run + float dt = (float)(hrt_absolute_time() - _last_run) / 1000000.f; + _last_run = hrt_absolute_time(); - // perform prior update assuming trivial dynamics of the wind field (mean field stays the same) - _X_prior_horizontal = _X_posterior_horizontal; - _X_prior_vertical = _X_posterior_vertical; - _P_prior_horizontal = _P_posterior_horizontal + dt*_Q_horizontal; - _P_prior_vertical = _P_posterior_vertical + dt*_Q_vertical; + if (dt > 1.f) { + dt = 1.f; + } + + // perform prior update assuming trivial dynamics of the wind field (mean field stays the same) + _X_prior_horizontal = _X_posterior_horizontal; + _X_prior_vertical = _X_posterior_vertical; + _P_prior_horizontal = _P_posterior_horizontal + dt * _Q_horizontal; + _P_prior_vertical = _P_posterior_vertical + dt * _Q_vertical; } void FixedwingShearEstimator::perform_posterior_update(float height, Vector3f wind) { - // - bool error = false; + // + bool error = false; - // first fill the horizontal observation matrix: - float vx = _X_prior_horizontal(0); - float vy = _X_prior_horizontal(1); - float h = _X_prior_horizontal(4); - float a = _X_prior_horizontal(5); + // first fill the horizontal observation matrix: + float vx = _X_prior_horizontal(0); + float vy = _X_prior_horizontal(1); + float h = _X_prior_horizontal(4); + float a = _X_prior_horizontal(5); - _H_horizontal.setZero(); - _H_horizontal(0,0) = 1.f/(1.f + expf(-(height-h)*a)); - _H_horizontal(1,1) = 1.f/(1.f + expf(-(height-h)*a)); - _H_horizontal(0,2) = 1.f; - _H_horizontal(1,3) = 1.f; - _H_horizontal(0,4) = -((a*vx*expf(-(height-h)*a)))/powf((1.f + expf(-(height-h)*a)),2.f); - _H_horizontal(1,4) = -((a*vy*expf(-(height-h)*a)))/powf((1.f + expf(-(height-h)*a)),2.f); - _H_horizontal(0,5) = -((vx*(h-height))*expf(-(height-h)*a))/powf((1.f + expf(-(height-h)*a)),2.f); - _H_horizontal(1,5) = -((vy*(h-height))*expf(-(height-h)*a))/powf((1.f + expf(-(height-h)*a)),2.f); + _H_horizontal.setZero(); + _H_horizontal(0, 0) = 1.f / (1.f + expf(-(height - h) * a)); + _H_horizontal(1, 1) = 1.f / (1.f + expf(-(height - h) * a)); + _H_horizontal(0, 2) = 1.f; + _H_horizontal(1, 3) = 1.f; + _H_horizontal(0, 4) = -((a * vx * expf(-(height - h) * a))) / powf((1.f + expf(-(height - h) * a)), 2.f); + _H_horizontal(1, 4) = -((a * vy * expf(-(height - h) * a))) / powf((1.f + expf(-(height - h) * a)), 2.f); + _H_horizontal(0, 5) = -((vx * (h - height)) * expf(-(height - h) * a)) / powf((1.f + expf(-(height - h) * a)), 2.f); + _H_horizontal(1, 5) = -((vy * (h - height)) * expf(-(height - h) * a)) / powf((1.f + expf(-(height - h) * a)), 2.f); - // then fill the vertical observation matrix - for (uint i=0;i<_dim_vertical;i++){ - _H_vertical(0,i) = powf(height-h,i); - } + // then fill the vertical observation matrix + for (uint i = 0; i < _dim_vertical; i++) { + _H_vertical(0, i) = powf(height - h, i); + } - // compute Kalman gain matrix for horizontal wind states - Matrix tmp1_horizontal = _P_prior_horizontal*_H_horizontal.T(); - Matrix tmp2_horizontal = _H_horizontal*(_P_prior_horizontal*_H_horizontal.T()) + _R_horizontal; - float determinant_horizontal = tmp2_horizontal(0,0)*tmp2_horizontal(1,1) - tmp2_horizontal(1,0)*tmp2_horizontal(0,1); - if ((float)fabs(determinant_horizontal)>=0.000001f){ - _K_horizontal = tmp1_horizontal*geninv(tmp2_horizontal); - } - else{ - //PX4_WARN("singular horizontal matrix, resetting filter"); - error = true; - } + // compute Kalman gain matrix for horizontal wind states + Matrix tmp1_horizontal = _P_prior_horizontal * _H_horizontal.T(); + Matrix tmp2_horizontal = _H_horizontal * (_P_prior_horizontal * _H_horizontal.T()) + _R_horizontal; + float determinant_horizontal = tmp2_horizontal(0, 0) * tmp2_horizontal(1, 1) - tmp2_horizontal(1, + 0) * tmp2_horizontal(0, 1); - // then fill the vertical observation matrix - for (uint i=0;i<_dim_vertical;i++){ - _H_vertical(0,i) = powf(height-h,i); - } + if ((float)fabs(determinant_horizontal) >= 0.000001f) { + _K_horizontal = tmp1_horizontal * geninv(tmp2_horizontal); - // compute Kalman gain matrix for vertical wind states - Matrix tmp1_vertical = _P_prior_vertical*_H_vertical.T(); - Matrix tmp2_vertical = _H_vertical*_P_prior_vertical*_H_vertical.T() + _R_vertical; - Matrix inv_vertical; - float determinant_vertical = tmp2_vertical(0,0); - if ((float)fabs(determinant_vertical)>0.000001f){ - inv_vertical(0,0) = 1.f / determinant_vertical; - _K_vertical = tmp1_vertical*inv_vertical; - } - else{ - //PX4_WARN("singular vertical matrix, resetting filter"); - error = true; - } + } else { + //PX4_WARN("singular horizontal matrix, resetting filter"); + error = true; + } - if (error) { - reset_filter(); - } - else { - // perform horizontal update - Vector z_expected_horizontal; - Vector wind_horizontal; - Matrix identity_1; - z_expected_horizontal(0) = _X_prior_horizontal(0)/(1.f+expf(-_X_prior_horizontal(5)*(_current_height-_X_prior_horizontal(4)))) + _X_prior_horizontal(2); - z_expected_horizontal(1) = _X_prior_horizontal(1)/(1.f+expf(-_X_prior_horizontal(5)*(_current_height-_X_prior_horizontal(4)))) + _X_prior_horizontal(3); - wind_horizontal(0) = _current_wind(0); - wind_horizontal(1) = _current_wind(1); - identity_1.setIdentity(); - _X_posterior_horizontal = _X_prior_horizontal + _K_horizontal*(wind_horizontal - z_expected_horizontal); - _P_posterior_horizontal = (identity_1 - _K_horizontal*_H_horizontal)*_P_prior_horizontal; + // then fill the vertical observation matrix + for (uint i = 0; i < _dim_vertical; i++) { + _H_vertical(0, i) = powf(height - h, i); + } - // perform vertical update - Vector z_expected_vertical = (Vector) (_H_vertical*_X_prior_vertical); - Vector wind_vertical; - Matrix identity_2; - wind_vertical(0) = wind(2); - identity_2.setIdentity(); - _X_posterior_vertical = _X_prior_vertical + _K_vertical*(wind_vertical - z_expected_vertical); - _P_posterior_vertical = (identity_2 - _K_vertical*_H_vertical)*_P_prior_vertical; - } + // compute Kalman gain matrix for vertical wind states + Matrix tmp1_vertical = _P_prior_vertical * _H_vertical.T(); + Matrix tmp2_vertical = _H_vertical * _P_prior_vertical * _H_vertical.T() + _R_vertical; + Matrix inv_vertical; + float determinant_vertical = tmp2_vertical(0, 0); - // find the correct sign of params (parametrization is not unique) - if (_X_posterior_horizontal(5)<0.f) { - _X_posterior_horizontal(0) *= -1.f; - _X_posterior_horizontal(1) *= -1.f; - _X_posterior_horizontal(2) = _X_posterior_horizontal(2) - _X_posterior_horizontal(0); - _X_posterior_horizontal(3) = _X_posterior_horizontal(3) - _X_posterior_horizontal(1); - _X_posterior_horizontal(5) *= -1.f; - } + if ((float)fabs(determinant_vertical) > 0.000001f) { + inv_vertical(0, 0) = 1.f / determinant_vertical; + _K_vertical = tmp1_vertical * inv_vertical; + + } else { + //PX4_WARN("singular vertical matrix, resetting filter"); + error = true; + } + + if (error) { + reset_filter(); + + } else { + // perform horizontal update + Vector z_expected_horizontal; + Vector wind_horizontal; + Matrix identity_1; + z_expected_horizontal(0) = _X_prior_horizontal(0) / (1.f + expf(-_X_prior_horizontal(5) * + (_current_height - _X_prior_horizontal(4)))) + _X_prior_horizontal(2); + z_expected_horizontal(1) = _X_prior_horizontal(1) / (1.f + expf(-_X_prior_horizontal(5) * + (_current_height - _X_prior_horizontal(4)))) + _X_prior_horizontal(3); + wind_horizontal(0) = _current_wind(0); + wind_horizontal(1) = _current_wind(1); + identity_1.setIdentity(); + _X_posterior_horizontal = _X_prior_horizontal + _K_horizontal * (wind_horizontal - z_expected_horizontal); + _P_posterior_horizontal = (identity_1 - _K_horizontal * _H_horizontal) * _P_prior_horizontal; + + // perform vertical update + Vector z_expected_vertical = (Vector)(_H_vertical * _X_prior_vertical); + Vector wind_vertical; + Matrix identity_2; + wind_vertical(0) = wind(2); + identity_2.setIdentity(); + _X_posterior_vertical = _X_prior_vertical + _K_vertical * (wind_vertical - z_expected_vertical); + _P_posterior_vertical = (identity_2 - _K_vertical * _H_vertical) * _P_prior_vertical; + } + + // find the correct sign of params (parametrization is not unique) + if (_X_posterior_horizontal(5) < 0.f) { + _X_posterior_horizontal(0) *= -1.f; + _X_posterior_horizontal(1) *= -1.f; + _X_posterior_horizontal(2) = _X_posterior_horizontal(2) - _X_posterior_horizontal(0); + _X_posterior_horizontal(3) = _X_posterior_horizontal(3) - _X_posterior_horizontal(1); + _X_posterior_horizontal(5) *= -1.f; + } } @@ -366,19 +382,19 @@ FixedwingShearEstimator::perform_posterior_update(float height, Vector3f wind) void FixedwingShearEstimator::Run() { - if (should_exit()) { + if (should_exit()) { _soaring_controller_wind_sub.unregisterCallback(); exit_and_cleanup(); return; } - perf_begin(_loop_perf); + perf_begin(_loop_perf); - // only run controller if wind info changed - soaring_controller_wind_s soaring_controller_wind; - if (_soaring_controller_wind_sub.update(&soaring_controller_wind)) - { - // only update parameters if they changed + // only run controller if wind info changed + soaring_controller_wind_s soaring_controller_wind; + + if (_soaring_controller_wind_sub.update(&soaring_controller_wind)) { + // only update parameters if they changed bool params_updated = _parameter_update_sub.updated(); // check for parameter updates @@ -391,194 +407,211 @@ FixedwingShearEstimator::Run() updateParams(); parameters_update(); } - // get current measurement - _current_wind = Vector3f(soaring_controller_wind.wind_estimate_filtered)/_unit_v; - _current_height = Vector3f(soaring_controller_wind.position)(2)/_unit_h; - _current_airspeed = soaring_controller_wind.airspeed; - _lock_params = soaring_controller_wind.lock_params; - //PX4_INFO("estimating shear, shear lock is: \t%d", _lock_params); - // only update shear estimae if aircraft is close to the soaring frame origin (shear is a local phenomenon) - if (soaring_controller_wind.valid) { - // prior update - perform_prior_update(); + // get current measurement + _current_wind = Vector3f(soaring_controller_wind.wind_estimate_filtered) / _unit_v; + _current_height = Vector3f(soaring_controller_wind.position)(2) / _unit_h; + _current_airspeed = soaring_controller_wind.airspeed; + _lock_params = soaring_controller_wind.lock_params; + //PX4_INFO("estimating shear, shear lock is: \t%d", _lock_params); - // posterior update - perform_posterior_update(_current_height, _current_wind); + // only update shear estimae if aircraft is close to the soaring frame origin (shear is a local phenomenon) + if (soaring_controller_wind.valid) { + // prior update + perform_prior_update(); - // check if filter diverges - // maybe reset filters... - if (sqrtf(_P_posterior_horizontal(4,4))>=5.f||sqrtf(_P_posterior_horizontal(5,5))>=0.3f) { - PX4_WARN("large height uncertainty, resetting filter"); - reset_filter(); - } + // posterior update + perform_posterior_update(_current_height, _current_wind); - } + // check if filter diverges + // maybe reset filters... + if (sqrtf(_P_posterior_horizontal(4, 4)) >= 5.f || sqrtf(_P_posterior_horizontal(5, 5)) >= 0.3f) { + PX4_WARN("large height uncertainty, resetting filter"); + reset_filter(); + } - // ===================================================== - // get the correct shear params for trajectory selection - // ===================================================== - 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(0), - _X_posterior_horizontal(1)); + } + + // ===================================================== + // get the correct shear params for trajectory selection + // ===================================================== + 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(0), + _X_posterior_horizontal(1)); - // only update all params for trajectory selection if required to do so - if (!_lock_params) { - // get the appropriate airspeed for trajectory selection - float min_aspd = _MIN_ASPD_MATRIX(round(v-8.f),round(10.f*(a-0.2f))); - float max_aspd = _MAX_ASPD_MATRIX(round(v-8.f),round(10.f*(a-0.2f))); - uint num = (uint)round((max_aspd-min_aspd)/2.f + 1.f); - float aspd_arr[num] = {}; - for (uint i=0; i3.f) { - return false; - } + // require shear strength above 0.2 1/s + if (shear_strength < 0.2f) { + return false; + } - return true; + // require covariance certainty below 2 m/s (use propagation of variance) + float shear_stdev = sqrtf(powf(shear_x / shear, 2) * _P_posterior_horizontal(0, 0) + + powf(shear_x / shear, 2) * _P_posterior_horizontal(2, 2) + + powf(shear_y / shear, 2) * _P_posterior_horizontal(1, 1) + + powf(shear_y / shear, 2) * _P_posterior_horizontal(3, 3)); + + if (shear_stdev > 3.f) { + return false; + } + + return true; } float FixedwingShearEstimator::_getClosest(float val1, float val2, float target) { - // ============================================================================= - // METHOD COPIED FROM "https://www.geeksforgeeks.org/find-closest-number-array/" - // ============================================================================= - if (target - val1 >= val2 - target) - return val2; - else - return val1; + // ============================================================================= + // METHOD COPIED FROM "https://www.geeksforgeeks.org/find-closest-number-array/" + // ============================================================================= + if (target - val1 >= val2 - target) { + return val2; + + } else { + return val1; + } } float FixedwingShearEstimator::_findClosest(float arr[], int n, float target) { - // ============================================================================= - // METHOD COPIED FROM "https://www.geeksforgeeks.org/find-closest-number-array/" - // ============================================================================= - // Corner cases - if (target <= arr[0]) - return arr[0]; - if (target >= arr[n - 1]) - return arr[n - 1]; - - // Doing binary search - int i = 0, j = n, mid = 0; - while (i < j) { - mid = (i + j) / 2; - - if ((float)fabs(arr[mid]-target) < (float)0.0001f) - return arr[mid]; - - /* If target is less than array element, - then search in left */ - if (target < arr[mid]) { - - // If target is greater than previous - // to mid, return closest of two - if (mid > 0 && target > arr[mid - 1]) - return _getClosest(arr[mid - 1], - arr[mid], target); - - /* Repeat for left half */ - j = mid; - } - - // If target is greater than mid - else { - if (mid < n - 1 && target < arr[mid + 1]) - return _getClosest(arr[mid], - arr[mid + 1], target); - // update i - i = mid + 1; - } - } - - // Only single element left after search - return arr[mid]; + // ============================================================================= + // METHOD COPIED FROM "https://www.geeksforgeeks.org/find-closest-number-array/" + // ============================================================================= + // Corner cases + if (target <= arr[0]) { + return arr[0]; + } + + if (target >= arr[n - 1]) { + return arr[n - 1]; + } + + // Doing binary search + int i = 0, j = n, mid = 0; + + while (i < j) { + mid = (i + j) / 2; + + if ((float)fabs(arr[mid] - target) < (float)0.0001f) { + return arr[mid]; + } + + /* If target is less than array element, + then search in left */ + if (target < arr[mid]) { + + // If target is greater than previous + // to mid, return closest of two + if (mid > 0 && target > arr[mid - 1]) + return _getClosest(arr[mid - 1], + arr[mid], target); + + /* Repeat for left half */ + j = mid; + } + + // If target is greater than mid + else { + if (mid < n - 1 && target < arr[mid + 1]) + return _getClosest(arr[mid], + arr[mid + 1], target); + + // update i + i = mid + 1; + } + } + + // Only single element left after search + return arr[mid]; } @@ -608,10 +641,11 @@ int FixedwingShearEstimator::task_spawn(int argc, char *argv[]) int FixedwingShearEstimator::custom_command(int argc, char *argv[]) { - if (!strcmp(argv[0], "reset")) { - get_instance()->reset_filter(); + if (!strcmp(argv[0], "reset")) { + get_instance()->reset_filter(); return 0; } + return print_usage("unknown command"); } @@ -630,7 +664,7 @@ int FixedwingShearEstimator::print_usage(const char *reason) PRINT_MODULE_USAGE_NAME("fw_dyn_soar_estimator", "estimator"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true); - PRINT_MODULE_USAGE_COMMAND("reset"); + PRINT_MODULE_USAGE_COMMAND("reset"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; diff --git a/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.hpp b/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.hpp index 958b4bb039..232f4165f1 100644 --- a/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.hpp +++ b/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.hpp @@ -66,90 +66,90 @@ private: uORB::SubscriptionCallbackWorkItem _soaring_controller_wind_sub{this, ORB_ID(soaring_controller_wind)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - - // Publishers + + // Publishers uORB::Publication _soaring_estimator_shear_pub{ORB_ID(soaring_estimator_shear)}; - // Message structs - soaring_estimator_shear_s _soaring_estimator_shear{}; ///< soaring controller pos + // Message structs + soaring_estimator_shear_s _soaring_estimator_shear{}; ///< soaring controller pos soaring_controller_wind_s _soaring_controller_wind{}; ///< soaring controller wind // parameter struct DEFINE_PARAMETERS( // aircraft params (ParamFloat) _param_sigma_q_vel, - (ParamFloat) _param_sigma_q_h, - (ParamFloat) _param_sigma_q_a, - (ParamFloat) _param_sigma_r_vel, - (ParamFloat) _param_init_h + (ParamFloat) _param_sigma_q_h, + (ParamFloat) _param_sigma_q_a, + (ParamFloat) _param_sigma_r_vel, + (ParamFloat) _param_init_h ) perf_counter_t _loop_perf; ///< loop performance counter - + // Update our local parameter cache. int parameters_update(); - void reset_filter(); - void perform_prior_update(); - void perform_posterior_update(float height, Vector3f wind); - bool check_feasibility(); - void publish_estimate(); - void status_publish(); + void reset_filter(); + void perform_prior_update(); + void perform_posterior_update(float height, Vector3f wind); + bool check_feasibility(); + void publish_estimate(); + void status_publish(); - // helper methods + // helper methods float _getClosest(float val1, float val2, float taget); // get float closest to target float _findClosest(float arr[], int n, float target); // return element in arr closest to n - // control variables - hrt_abstime _last_run{0}; - uint _reset_counter = {}; - const static size_t _dim_vertical = 1; // order of vertical approximation function for vertical wind + // control variables + hrt_abstime _last_run{0}; + uint _reset_counter = {}; + const static size_t _dim_vertical = 1; // order of vertical approximation function for vertical wind - Vector _X_prior_horizontal= {}; - Matrix _P_prior_horizontal = {}; - Vector _X_posterior_horizontal= {}; - Matrix _P_posterior_horizontal = {}; - Matrix _Q_horizontal = {}; - Matrix _R_horizontal = {}; - Matrix _H_horizontal = {}; - Matrix _A_horizontal = {}; - Matrix _K_horizontal = {}; + Vector _X_prior_horizontal = {}; + Matrix _P_prior_horizontal = {}; + Vector _X_posterior_horizontal = {}; + Matrix _P_posterior_horizontal = {}; + Matrix _Q_horizontal = {}; + Matrix _R_horizontal = {}; + Matrix _H_horizontal = {}; + Matrix _A_horizontal = {}; + Matrix _K_horizontal = {}; - Vector _X_prior_vertical= {}; - Matrix _P_prior_vertical = {}; - Vector _X_posterior_vertical= {}; - Matrix _P_posterior_vertical = {}; - Vector _X_vertical = {}; // params of vertical wind - Matrix _Q_vertical = {}; - Matrix _R_vertical = {}; - Matrix _H_vertical = {}; - Matrix _A_vertical = {}; - Matrix _K_vertical = {}; + Vector _X_prior_vertical = {}; + Matrix _P_prior_vertical = {}; + Vector _X_posterior_vertical = {}; + Matrix _P_posterior_vertical = {}; + Vector _X_vertical = {}; // params of vertical wind + Matrix _Q_vertical = {}; + Matrix _R_vertical = {}; + Matrix _H_vertical = {}; + Matrix _A_vertical = {}; + Matrix _K_vertical = {}; - // measurement variables - Vector3f _current_wind = {}; - float _current_height = {}; - float _current_airspeed = {}; - bool _lock_params = {}; - float _v_max_lock = {}; - float _alpha_lock = {}; + // measurement variables + Vector3f _current_wind = {}; + float _current_height = {}; + float _current_airspeed = {}; + bool _lock_params = {}; + float _v_max_lock = {}; + float _alpha_lock = {}; - float _unit_v; - float _unit_h; - float _unit_a; + float _unit_v; + float _unit_h; + float _unit_a; - // helper variables - float _init_height = {}; + // helper variables + float _init_height = {}; - // trajectory selection variables - // vectors defining the gridding for trajectory selection: initial velocities, wind speed and shear strength - float _v_max_arr[5] = {8.f,9.f,10.f,11.f,12.f}; - float _alpha_arr[9] = {0.2f,0.3f,0.4f,0.5f,0.6f,0.7f,0.8f,0.9f,1.0f}; - const static size_t _num_v_max = 5; - const static size_t _num_alpha = 9; - Matrix _MIN_ASPD_MATRIX = {}; - Matrix _MAX_ASPD_MATRIX = {}; + // trajectory selection variables + // vectors defining the gridding for trajectory selection: initial velocities, wind speed and shear strength + float _v_max_arr[5] = {8.f, 9.f, 10.f, 11.f, 12.f}; + float _alpha_arr[9] = {0.2f, 0.3f, 0.4f, 0.5f, 0.6f, 0.7f, 0.8f, 0.9f, 1.0f}; + const static size_t _num_v_max = 5; + const static size_t _num_alpha = 9; + Matrix _MIN_ASPD_MATRIX = {}; + Matrix _MAX_ASPD_MATRIX = {}; }; 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 11dca4d71e..71bc4b0c67 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 @@ -12,9 +12,9 @@ /** * Standard deviation of velicity state in shear model - * + * * This is the std dev of the wind velocity in each direction - * + * * @unit m * @min 0.0 * @max 10 @@ -26,9 +26,9 @@ PARAM_DEFINE_FLOAT(DS_SIGMA_Q_V, 0.3f); /** * Standard deviation of vertical shear position - * + * * This is the std dev of the shear vertical position - * + * * @unit m * @min 0.0 * @max 10 @@ -40,9 +40,9 @@ PARAM_DEFINE_FLOAT(DS_SIGMA_Q_H, 0.3f); /** * Standard deviation of velicity state in shear model - * + * * This is the std dev of the shear strenght param - * + * * @unit m * @min 0.0 * @max 10 @@ -54,9 +54,9 @@ PARAM_DEFINE_FLOAT(DS_SIGMA_Q_A, 0.01f); /** * Standard deviation of velicity measurement (wind) - * + * * This is the std dev of the wind pseudomeasurement passed to the EKF - * + * * @unit m * @min 0.0 * @max 10 @@ -68,9 +68,9 @@ PARAM_DEFINE_FLOAT(DS_SIGMA_R_V, 3.f); /** * Initial guess of the vertical shear position - * + * * This is the filter initial value of the shear vertical position - * + * * @unit m * @min -200 * @max 200