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

backup push

Browse Source
This commit is contained in:
Marvin Harms
2022-04-12 13:38:31 +02:00
parent 90bcdcf9e1
commit 2c9ff19e4f
2 changed files with 104 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
+92 -18
View File
@@ -53,7 +53,7 @@ using matrix::wrap_pi;
FixedwingPositionINDIControl::FixedwingPositionINDIControl() :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
_alpha_sp_pub(ORB_ID(vehicle_angular_acceleration_setpoint)),
_angular_accel_sp_pub(ORB_ID(vehicle_angular_acceleration_setpoint)),
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
{
// limit to 50 Hz
@@ -200,11 +200,10 @@ FixedwingPositionINDIControl::airflow_slip_poll()
void
FixedwingPositionINDIControl::vehicle_attitude_poll()
{
vehicle_attitude_s att;
if (_vehicle_attitude_sub.update(&att)) {
_att = Quatf(att.q);
if (_vehicle_attitude_sub.update(&_attitude)) {
_att = Quatf(_attitude.q);
}
if(att.timestamp_sample-hrt_absolute_time() > 50_ms){
if(hrt_absolute_time()-_attitude.timestamp_sample > 50_ms){
PX4_ERR("attitude sample is too old");
}
}
@@ -212,11 +211,10 @@ FixedwingPositionINDIControl::vehicle_attitude_poll()
void
FixedwingPositionINDIControl::vehicle_angular_velocity_poll()
{
vehicle_angular_velocity_s omega;
if (_vehicle_angular_velocity_sub.update(&omega)) {
_omega = Vector3f(omega.xyz);
if (_vehicle_angular_velocity_sub.update(&_angular_vel)) {
_omega = Vector3f(_angular_vel.xyz);
}
if(omega.timestamp_sample-hrt_absolute_time() > 50_ms){
if(hrt_absolute_time()-_angular_vel.timestamp_sample > 50_ms){
PX4_ERR("angular velocity sample is too old");
}
}
@@ -224,11 +222,10 @@ FixedwingPositionINDIControl::vehicle_angular_velocity_poll()
void
FixedwingPositionINDIControl::vehicle_angular_acceleration_poll()
{
vehicle_angular_acceleration_s alpha;
if (_vehicle_angular_acceleration_sub.update(&alpha)) {
_alpha = Vector3f(alpha.xyz);
if (_vehicle_angular_acceleration_sub.update(&_angular_accel)) {
_alpha = Vector3f(_angular_accel.xyz);
}
if(alpha.timestamp_sample-hrt_absolute_time() > 50_ms){
if(hrt_absolute_time()-_angular_accel.timestamp_sample > 50_ms){
PX4_ERR("angular acceleration sample is too old");
}
}
@@ -262,6 +259,73 @@ FixedwingPositionINDIControl::_set_wind_estimate(Vector3f wind)
return;
}
void
FixedwingPositionINDIControl::_read_trajectory_coeffs_csv()
{
/*
std::ifstream file_reader;
std::string line;
std::vector<float> vars;
file_reader.open(model_input_mean_path_, std::ifstream::in);
for (int i = 0; i < _num_basis_funs; i++){
file_reader >> line;
_basis_coeffs_x(i) = std::stof(line);
}
file_reader.close();
*/
_basis_coeffs_x(0) = -0.026399f;
_basis_coeffs_x(0) = 826.714294f;
_basis_coeffs_x(2) = -3432.814434f;
_basis_coeffs_x(3) = 7863.143292f;
_basis_coeffs_x(4) = -13322.886901f;
_basis_coeffs_x(5) = 18316.582356f;
_basis_coeffs_x(6) = -21634.179996f;
_basis_coeffs_x(7) = 22466.018036f;
_basis_coeffs_x(8) = -20949.604526f;
_basis_coeffs_x(9) = 17621.148149f;
_basis_coeffs_x(10) = -13520.515123f;
_basis_coeffs_x(11) = 9422.542947f;
_basis_coeffs_x(12) = -6018.861971f;
_basis_coeffs_x(13) = 3385.119029f;
_basis_coeffs_x(14) = -1600.362985f;
_basis_coeffs_x(15) = 439.235977f;
_basis_coeffs_y(0) = -0.139214f;
_basis_coeffs_y(1) = 1528.730954f;
_basis_coeffs_y(2) = -6325.861755f;
_basis_coeffs_y(3) = 15443.549565f;
_basis_coeffs_y(4) = -28040.238622f;
_basis_coeffs_y(5) = 41980.028597f;
_basis_coeffs_y(6) = -54384.100803f;
_basis_coeffs_y(7) = 62686.013646f;
_basis_coeffs_y(8) = -65075.809430f;
_basis_coeffs_y(9) = 61050.185712f;
_basis_coeffs_y(10) = -51403.496086f;
_basis_coeffs_y(11) = 38173.744062f;
_basis_coeffs_y(12) = -24154.989309f;
_basis_coeffs_y(13) = 12186.703620f;
_basis_coeffs_y(14) = -4286.372935f;
_basis_coeffs_y(15) = 679.830536f;
_basis_coeffs_z(0) = 4.971065f;
_basis_coeffs_z(1) = -354.548028f;
_basis_coeffs_z(2) = 1506.974164f;
_basis_coeffs_z(3) = -3506.606108f;
_basis_coeffs_z(4) = 5944.806897f;
_basis_coeffs_z(5) = -8122.670014f;
_basis_coeffs_z(6) = 9471.905195f;
_basis_coeffs_z(7) = -9711.318513f;
_basis_coeffs_z(8) = 8958.890412f;
_basis_coeffs_z(9) = -7501.461083f;
_basis_coeffs_z(10) = 5797.319144f;
_basis_coeffs_z(11) = -4126.927355f;
_basis_coeffs_z(12) = 2727.759847f;
_basis_coeffs_z(13) = -1570.235132f;
_basis_coeffs_z(14) = 727.014138f;
_basis_coeffs_z(15) = -189.177675;
}
void
FixedwingPositionINDIControl::Run()
{
@@ -304,13 +368,21 @@ FixedwingPositionINDIControl::Run()
// 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);
_alpha_sp_pub.publish(_angular_accel_sp);
_angular_accel_sp_pub.publish(_angular_accel_sp);
//print_message(_angular_accel_sp);
//PX4_INFO("running");
/*
Quatf vec = _get_attitude_ref(0.2,1);
PX4_INFO("control setpoints:\t%.4f\t%.4f\t%.4f",
(double)vec(0),
(double)vec(1),
(double)vec(2));
*/
}
}
@@ -416,7 +488,7 @@ FixedwingPositionINDIControl::_get_attitude_ref(float t, float T)
R_bi(2,2) = lift_normalized(2);
// compute required AoA
Vector3f f_phi = R_bi*f_lift;
float AoA = ((2.f*f_phi(2))/(_rho*_area*(vel_air*vel_air)) - _C_L0)/_C_L1;
float AoA = ((2.f*f_phi(2))/(_rho*_area*(vel_air*vel_air)+0.001f) - _C_L0)/_C_L1;
// compute final rotation matrix
Eulerf e(0.f, AoA, 0.f);
Dcmf R_pitch(e);
@@ -531,10 +603,11 @@ FixedwingPositionINDIControl::_compute_NDI_control_input(Vector3f pos, Vector3f
Dcmf R_bi(R_ib.transpose());
// get reference values
float t_ref = _get_closest_t(pos);
PX4_INFO("computed t_ref:\t%.4f",(double)t_ref);
// 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.f);
float T = sqrt((v_ref_*v_ref_)/(vel*vel+0.01f));
float T = sqrt((v_ref_*v_ref_)/(vel*vel+0.001f));
// get time-scaled version of reference trajectory
Vector3f x_ref = _get_position_ref(t_ref);
Vector3f v_ref = _get_velocity_ref(t_ref,T);
@@ -592,6 +665,7 @@ FixedwingPositionINDIControl::init()
PX4_ERR("vehicle position callback registration failed!");
return false;
}
_read_trajectory_coeffs_csv();
return true;
}
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp
+12 -6
View File
@@ -53,6 +53,8 @@
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/wind.h>
#include <uORB/uORB.h>
#include <iostream>
#include <fstream>
using namespace time_literals;
@@ -106,13 +108,16 @@ private:
// Publishers
uORB::Publication<vehicle_angular_acceleration_setpoint_s> _alpha_sp_pub;
uORB::Publication<vehicle_angular_acceleration_setpoint_s> _angular_accel_sp_pub;
// Message structs
vehicle_angular_acceleration_setpoint_s _angular_accel_sp {};
manual_control_setpoint_s _manual_control_setpoint {}; ///< r/c channel data
vehicle_control_mode_s _control_mode {}; ///< control mode
vehicle_local_position_s _local_pos {}; ///< vehicle local position
vehicle_attitude_s _attitude {}; ///< vehicle attitude
vehicle_angular_velocity_s _angular_vel {}; ///< vehicle angular velocity
vehicle_angular_acceleration_s _angular_accel {}; ///< vehicle angular acceleration
vehicle_control_mode_s _control_mode {}; ///< control mode
vehicle_status_s _vehicle_status {}; ///< vehicle status
// parameter struct
@@ -184,9 +189,10 @@ private:
void status_publish();
const int _num_points = 30; // number of points on the precomputed trajectory
const static size_t _num_basis_funs = 15; // number of basis functions used for the trajectory approximation
const static size_t _num_basis_funs = 16; // number of basis functions used for the trajectory approximation
// controller methods
void _read_trajectory_coeffs_csv(); // read in the correct coefficients of the appropriate trajectory
void _set_wind_estimate(Vector3f wind);
float _get_closest_t(Vector3f pos); // get the normalized time, at which the reference path is closest to the current position
Vector<float, _num_basis_funs> _get_basis_funs(float t=0); // compute the vector of basis functions at normalized time t in [0,1]
@@ -204,9 +210,9 @@ private:
Vector3f _compute_INDI_control_input(Vector3f pos, Vector3f vel, Vector3f acc, Quatf att, Vector3f omega, Vector3f alpha);
// control variables
Vector<float, _num_basis_funs> _basis_coeffs_x; // coefficients of the current path
Vector<float, _num_basis_funs> _basis_coeffs_y; // coefficients of the current path
Vector<float, _num_basis_funs> _basis_coeffs_z; // coefficients of the current path
Vector<float, _num_basis_funs> _basis_coeffs_x = {}; // coefficients of the current path
Vector<float, _num_basis_funs> _basis_coeffs_y = {}; // coefficients of the current path
Vector<float, _num_basis_funs> _basis_coeffs_z = {}; // coefficients of the current path
Vector3f _alpha_sp;
Vector3f _wind_estimate;
Matrix3f _K_x;