fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-11 04:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

added trajecotry origin location params

Browse Source
This commit is contained in:
Marvin Harms
2022-05-01 11:51:31 +02:00
parent e6445db239
commit 2886705d06
4 changed files with 102 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/rc.fw_apps
+2
View File
@@ -21,3 +21,5 @@ airspeed_selector start
# Start Land Detector.
#
land_detector start fixedwing
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
+30 -3
View File
@@ -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);
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp
+16 -1
View File
@@ -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<px4::params::C_D0>) _param_fw_c_d0,
(ParamFloat<px4::params::C_D1>) _param_fw_c_d1,
(ParamFloat<px4::params::C_D2>) _param_fw_c_d2,
(ParamFloat<px4::params::AOA_OFFSET>) _param_aoa_offset,
// filter params
(ParamFloat<px4::params::FILTER_A1>) _param_filter_a1,
(ParamFloat<px4::params::FILTER_A2>) _param_filter_a2,
@@ -184,7 +186,11 @@ private:
(ParamFloat<px4::params::K_W_YAW>) _param_k_w_yaw,
(ParamFloat<px4::params::K_ACT_ROLL>) _param_k_act_roll,
(ParamFloat<px4::params::K_ACT_PITCH>) _param_k_act_pitch,
(ParamFloat<px4::params::K_ACT_YAW>) _param_k_act_yaw
(ParamFloat<px4::params::K_ACT_YAW>) _param_k_act_yaw,
// location params
(ParamFloat<px4::params::ORIGIN_LAT>) _param_origin_lat,
(ParamFloat<px4::params::ORIGIN_LON>) _param_origin_lon,
(ParamFloat<px4::params::ORIGIN_ALT>) _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.
src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c
+54
View File
@@ -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);