diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index feeda50914..444f536939 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -21,3 +21,5 @@ airspeed_selector start # Start Land Detector. # land_detector start fixedwing + + diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index 6492053557..e1c58cf9cc 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -127,6 +127,7 @@ FixedwingPositionINDIControl::parameters_update() _C_D0 = _param_fw_c_d0.get(); _C_D1 = _param_fw_c_d1.get(); _C_D2 = _param_fw_c_d2.get(); + _aoa_offset = _param_aoa_offset.get(); // filter parameters _a1 = _param_filter_a1.get(); @@ -141,6 +142,15 @@ FixedwingPositionINDIControl::parameters_update() _K_actuators(1,1) = _param_k_act_pitch.get(); _K_actuators(2,2) = _param_k_act_yaw.get(); + // trajectory origin + _origin_lat = _param_origin_lat.get(); + _origin_lon = _param_origin_lon.get(); + _origin_alt = _param_origin_alt.get(); + if (map_projection_project(&_global_local_proj_ref, _origin_lat, _origin_lon, &_origin_E, &_origin_N)) { + // TODO: do stuff + } + + // sanity check parameters // TODO: include sanity check @@ -284,6 +294,9 @@ FixedwingPositionINDIControl::vehicle_local_position_poll() _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}; _acc = _R_ned_to_enu*Vector3f{_local_pos.ax,_local_pos.ay,_local_pos.az}; + // transform to soaring frame + _pos = _pos - _R_ned_to_enu * Vector3f{_origin_N, _origin_E, _origin_D}; + //PX4_INFO("local position:\t%.4f\t%.4f\t%.4f", (double)_pos(0),(double)_pos(1),(double)_pos(2)); //PX4_INFO("local velocity:\t%.4f\t%.4f\t%.4f", (double)_vel(0),(double)_vel(1),(double)_vel(2)); //PX4_INFO("local acceleration:\t%.4f\t%.4f\t%.4f", (double)_acc(0),(double)_acc(1),(double)_acc(2)); @@ -454,9 +467,22 @@ FixedwingPositionINDIControl::Run() updateParams(); parameters_update(); } + //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: + if (!map_projection_initialized(&_global_local_proj_ref) + || (_global_local_proj_ref.timestamp != _local_pos.ref_timestamp)) { + // 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_E, &_origin_N); + _origin_D = _local_pos.ref_alt - _origin_alt; + PX4_INFO("local reference frame updated"); + } + // run polls _set_wind_estimate(Vector3f(0.f,0.f,0.f)); vehicle_status_poll(); @@ -470,6 +496,7 @@ FixedwingPositionINDIControl::Run() vehicle_angular_acceleration_poll(); soaring_controller_status_poll(); + // ============================ // compute reference kinematics // ============================ @@ -689,7 +716,7 @@ FixedwingPositionINDIControl::_get_attitude_ref(float t, float T) R_bi.renormalize(); // compute required AoA Vector3f f_phi = R_bi*f_lift; - float AoA = ((2.f*f_phi(2))/(_rho*_area*(vel_air*vel_air)+0.001f) - _C_L0)/_C_L1; + float AoA = ((2.f*f_phi(2))/(_rho*_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); @@ -808,7 +835,7 @@ FixedwingPositionINDIControl::_get_attitude(Vector3f vel, Vector3f f) R_bi.renormalize(); // compute required AoA Vector3f f_phi = R_bi*f_lift; - float AoA = ((2.f*f_phi(2))/(_rho*_area*(vel_air*vel_air)+0.001f) - _C_L0)/_C_L1; + float AoA = ((2.f*f_phi(2))/(_rho*_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); @@ -843,7 +870,7 @@ FixedwingPositionINDIControl::_compute_NDI_stage_1(Vector3f pos_ref, Vector3f ve // ================================== Vector3f f_current; Vector3f vel_body = R_bi*_vel; - float AoA = atan2f(vel_body(2), vel_body(0)); + 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); float factor = -0.5f*_rho*_area*sqrtf(vel_body*vel_body); diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp index 182524f500..cc5da9a181 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp @@ -139,6 +139,7 @@ private: vehicle_rates_setpoint_s _angular_vel_sp {}; ///< vehicle angular velocity setpoint vehicle_angular_acceleration_s _angular_accel {}; ///< vehicle angular acceleration home_position_s _home_pos {}; ///< home position + map_projection_reference_s _global_local_proj_ref{}; vehicle_control_mode_s _control_mode {}; ///< control mode offboard_control_mode_s _offboard_control_mode {}; ///< offboard control mode vehicle_status_s _vehicle_status {}; ///< vehicle status @@ -160,6 +161,7 @@ private: (ParamFloat) _param_fw_c_d0, (ParamFloat) _param_fw_c_d1, (ParamFloat) _param_fw_c_d2, + (ParamFloat) _param_aoa_offset, // filter params (ParamFloat) _param_filter_a1, (ParamFloat) _param_filter_a2, @@ -184,7 +186,11 @@ private: (ParamFloat) _param_k_w_yaw, (ParamFloat) _param_k_act_roll, (ParamFloat) _param_k_act_pitch, - (ParamFloat) _param_k_act_yaw + (ParamFloat) _param_k_act_yaw, + // location params + (ParamFloat) _param_origin_lat, + (ParamFloat) _param_origin_lon, + (ParamFloat) _param_origin_alt ) @@ -286,11 +292,20 @@ private: float _C_D0; float _C_D1; float _C_D2; + float _aoa_offset; float _a1; float _a2; float _b1; float _b2; float _b3; + // trajecotry origin in WGS84 + float _origin_lat; + float _origin_lon; + float _origin_alt; + // trajecotry origin in current NED local frame + float _origin_N; + float _origin_E; + float _origin_D; bool _airspeed_valid{false}; ///< flag if a valid airspeed estimate exists hrt_abstime _airspeed_last_valid{0}; ///< last time airspeed was received. Used to detect timeouts. 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 55538fc1a5..0e02e52eb2 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 @@ -169,6 +169,20 @@ PARAM_DEFINE_FLOAT(C_D1, 0.3783f); */ PARAM_DEFINE_FLOAT(C_D2, 1.984f); +/** + * offset angle between body frame (Pixhawk) and the wing chord + * + * Used to compute the AoA + * + * @unit rad + * @min 0 + * @max 0.1 + * @decimal 2 + * @increment 0.01 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(AOA_OFFSET, 0.01f); + /** * coefficients of the butterworth filter used for smoothing the IMU * @@ -447,3 +461,43 @@ PARAM_DEFINE_FLOAT(K_ACT_PITCH, 0.1f); * @group FW DYN SOAR Control */ PARAM_DEFINE_FLOAT(K_ACT_YAW, 0.1f); + +// =================================================== +// ============== trajectory center ================= +// =================================================== + +/** + * latitude of trajectory start point (WGS84) + * + * @unit + * @min -180 + * @max 180 + * @decimal 7 + * @increment 0.0000001 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(ORIGIN_LAT, 47.39797f); + +/** + * longitude of trajectory start point (WGS84) + * + * @unit + * @min -90 + * @max 90 + * @decimal 7 + * @increment 0.0000001 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(ORIGIN_LON, 8.54554f); + +/** + * altitude of trajectory start point (WGS84) + * + * @unit + * @min 0 + * @max 2000 + * @decimal 1 + * @increment 0.1 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(ORIGIN_ALT, 488.0f);