mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-17 05:23:34 +08:00
added basic init function wrappers
This commit is contained in:
Marvin Harms
@@ -68,6 +68,197 @@ FixedwingPositionINDIControl::~FixedwingPositionINDIControl()
|
||||
perf_free(_loop_perf);
|
||||
}
|
||||
|
||||
int
|
||||
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_k_x_roll.get();
|
||||
_K_x(1,1) = _param_k_x_pitch.get();
|
||||
_K_x(2,2) = _param_k_x_yaw.get();
|
||||
_K_v(0,0) = _param_k_v_roll.get();
|
||||
_K_v(1,1) = _param_k_v_pitch.get();
|
||||
_K_v(2,2) = _param_k_v_yaw.get();
|
||||
_K_a(0,0) = _param_k_a_roll.get();
|
||||
_K_a(1,1) = _param_k_a_pitch.get();
|
||||
_K_a(2,2) = _param_k_a_yaw.get();
|
||||
_K_q(0,0) = _param_k_q_roll.get();
|
||||
_K_q(1,1) = _param_k_q_pitch.get();
|
||||
_K_q(2,2) = _param_k_q_yaw.get();
|
||||
_K_w(0,0) = _param_k_w_roll.get();
|
||||
_K_w(1,1) = _param_k_w_pitch.get();
|
||||
_K_w(2,2) = _param_k_w_yaw.get();
|
||||
|
||||
// aircraft parameters
|
||||
_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();
|
||||
|
||||
// filter parameters
|
||||
_a1 = _param_filter_a1.get();
|
||||
_a2 = _param_filter_a2.get();
|
||||
_b1 = _param_filter_b1.get();
|
||||
_b2 = _param_filter_b2.get();
|
||||
_b3 = _param_filter_b3.get();
|
||||
|
||||
// sanity check parameters
|
||||
// TODO: include sanity check
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingPositionINDIControl::airspeed_poll()
|
||||
{
|
||||
bool airspeed_valid = _airspeed_valid;
|
||||
airspeed_validated_s airspeed_validated;
|
||||
|
||||
if (_airspeed_validated_sub.update(&airspeed_validated)) {
|
||||
|
||||
if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)
|
||||
&& PX4_ISFINITE(airspeed_validated.true_airspeed_m_s)
|
||||
&& (airspeed_validated.calibrated_airspeed_m_s > 0.0f)) {
|
||||
|
||||
airspeed_valid = true;
|
||||
|
||||
_airspeed_last_valid = airspeed_validated.timestamp;
|
||||
_airspeed = airspeed_validated.calibrated_airspeed_m_s;
|
||||
}
|
||||
|
||||
} else {
|
||||
// no airspeed updates for one second
|
||||
if (airspeed_valid && (hrt_elapsed_time(&_airspeed_last_valid) > 1_s)) {
|
||||
airspeed_valid = false;
|
||||
}
|
||||
}
|
||||
_airspeed_valid = airspeed_valid;
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingPositionINDIControl::airflow_aoa_poll()
|
||||
{
|
||||
bool aoa_valid = _aoa_valid;
|
||||
airflow_aoa_s aoa_validated;
|
||||
|
||||
if (_airflow_aoa_sub.update(&aoa_validated)) {
|
||||
|
||||
if (PX4_ISFINITE(aoa_validated.aoa_rad)
|
||||
&& (aoa_validated.valid)) {
|
||||
|
||||
aoa_valid = true;
|
||||
|
||||
_aoa_last_valid = aoa_validated.timestamp;
|
||||
_aoa = aoa_validated.aoa_rad;
|
||||
}
|
||||
|
||||
} else {
|
||||
// no aoa updates for one second
|
||||
if (aoa_valid && (hrt_elapsed_time(&_aoa_last_valid) > 1_s)) {
|
||||
aoa_valid = false;
|
||||
}
|
||||
}
|
||||
_aoa_valid = aoa_valid;
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingPositionINDIControl::airflow_slip_poll()
|
||||
{
|
||||
bool slip_valid = _slip_valid;
|
||||
airflow_slip_s slip_validated;
|
||||
|
||||
if (_airflow_slip_sub.update(&slip_validated)) {
|
||||
|
||||
if (PX4_ISFINITE(slip_validated.slip_rad)
|
||||
&& (slip_validated.valid)) {
|
||||
|
||||
slip_valid = true;
|
||||
|
||||
_slip_last_valid = slip_validated.timestamp;
|
||||
_slip = slip_validated.slip_rad;
|
||||
}
|
||||
|
||||
} else {
|
||||
// no aoa updates for one second
|
||||
if (slip_valid && (hrt_elapsed_time(&_slip_last_valid) > 1_s)) {
|
||||
slip_valid = false;
|
||||
}
|
||||
}
|
||||
_slip_valid = slip_valid;
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingPositionINDIControl::vehicle_attitude_poll()
|
||||
{
|
||||
vehicle_attitude_s att;
|
||||
if (_vehicle_attitude_sub.update(&att)) {
|
||||
_att = Quatf(att.q);
|
||||
}
|
||||
if(att.timestamp_sample-hrt_absolute_time() > 50_ms){
|
||||
PX4_ERR("attitude sample is too old");
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingPositionINDIControl::vehicle_angular_velocity_poll()
|
||||
{
|
||||
vehicle_angular_velocity_s omega;
|
||||
if (_vehicle_angular_velocity_sub.update(&omega)) {
|
||||
_omega = Vector3f(omega.xyz);
|
||||
}
|
||||
if(omega.timestamp_sample-hrt_absolute_time() > 50_ms){
|
||||
PX4_ERR("angular velocity sample is too old");
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingPositionINDIControl::vehicle_angular_acceleration_poll()
|
||||
{
|
||||
vehicle_angular_acceleration_s alpha;
|
||||
if (_vehicle_angular_acceleration_sub.update(&alpha)) {
|
||||
_alpha = Vector3f(alpha.xyz);
|
||||
}
|
||||
if(alpha.timestamp_sample-hrt_absolute_time() > 50_ms){
|
||||
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(&pos)) {
|
||||
_pos = Vector3f(pos.x,pos.y,pos.z);
|
||||
_vel = Vector3f(pos.vx,pos.vy,pos.vz);
|
||||
_acc = Vector3f(pos.ax,pos.ay,pos.az);
|
||||
}
|
||||
if(pos.timestamp_sample-hrt_absolute_time() > 50_ms){
|
||||
PX4_ERR("local position sample is too old");
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingPositionINDIControl::Run()
|
||||
{
|
||||
// only run controller if pos, vel, acc changed
|
||||
vehicle_local_position_s pos;
|
||||
|
||||
if (_vehicle_local_position_sub.update(&pos))
|
||||
{
|
||||
PX4_INFO("running");
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingPositionINDIControl::_set_wind_estimate(Vector3f wind)
|
||||
{
|
||||
@@ -158,7 +349,7 @@ FixedwingPositionINDIControl::_get_attitude_ref(float t, float T)
|
||||
// add gravity
|
||||
acc(2) += 9.81f;
|
||||
// compute required force
|
||||
Vector3f f = _param_fw_mass.get()*acc;
|
||||
Vector3f f = _mass*acc;
|
||||
// compute force component projected onto lift axis
|
||||
Vector3f vel_normalized = vel_air.normalized();
|
||||
Vector3f f_lift = f - f*vel_normalized;
|
||||
@@ -177,7 +368,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))/(_param_rho.get()*_param_fw_wing_area.get()*(vel_air*vel_air)) - _param_fw_c_l0.get())/_param_fw_c_l1.get();
|
||||
float AoA = ((2.f*f_phi(2))/(_rho*_area*(vel_air*vel_air)) - _C_L0)/_C_L1;
|
||||
// compute final rotation matrix
|
||||
Eulerf e(0.f, AoA, 0.f);
|
||||
Dcmf R_pitch(e);
|
||||
@@ -269,7 +460,7 @@ FixedwingPositionINDIControl::_get_attitude(Vector3f vel, Vector3f f)
|
||||
R_bi(2,2) = lift_normalized(2);
|
||||
// compute required AoA
|
||||
Vector3f f_phi = R_bi*f_lift;
|
||||
float AoA = ((2.f*f_phi(2))/(_param_rho.get()*_param_fw_wing_area.get()*(vel_air*vel_air)) - _param_fw_c_l0.get())/_param_fw_c_l1.get();
|
||||
float AoA = ((2.f*f_phi(2))/(_rho*_area*(vel_air*vel_air)) - _C_L0)/_C_L1;
|
||||
// compute final rotation matrix
|
||||
Eulerf e(0.f, AoA, 0.f);
|
||||
Dcmf R_pitch(e);
|
||||
@@ -307,7 +498,7 @@ FixedwingPositionINDIControl::_compute_NDI_control_input(Vector3f pos, Vector3f
|
||||
// add gravity
|
||||
acc_command(2) += 9.81f;
|
||||
// get force comand in world frame
|
||||
Vector3f f_command = _param_fw_mass.get()*acc_command;
|
||||
Vector3f f_command = _mass*acc_command;
|
||||
// get required attitude (assuming we can fly the target velocity)
|
||||
Dcmf R_ref(_get_attitude(v_ref,f_command));
|
||||
// get attitude error
|
||||
@@ -329,9 +520,39 @@ FixedwingPositionINDIControl::_compute_NDI_control_input(Vector3f pos, Vector3f
|
||||
return;
|
||||
}
|
||||
|
||||
int FixedwingPositionINDIControl::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
FixedwingPositionINDIControl *instance = new FixedwingPositionINDIControl();
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
|
||||
if (instance->init()) {
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("alloc failed");
|
||||
}
|
||||
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
bool
|
||||
FixedwingPositionINDIControl::init()
|
||||
{
|
||||
if (!_local_pos_sub.registerCallback()) {
|
||||
PX4_ERR("vehicle position callback registration failed!");
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
int FixedwingPositionINDIControl::custom_command(int argc, char *argv[])
|
||||
{
|
||||
|
||||
@@ -97,7 +97,7 @@ private:
|
||||
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_global_position_sub{ORB_ID(vehicle_global_position)}; // global position
|
||||
//uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; // global position
|
||||
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; // local NED position
|
||||
uORB::Subscription _vehicle_odometry_sub{ORB_ID(vehicle_odometry)}; // vehicle velocity
|
||||
uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; // vehicle acceleration
|
||||
@@ -123,7 +123,6 @@ private:
|
||||
(ParamFloat<px4::params::FW_WING_AREA>) _param_fw_wing_area,
|
||||
(ParamFloat<px4::params::RHO>) _param_rho,
|
||||
// aerodynamic params
|
||||
|
||||
(ParamFloat<px4::params::C_L0>) _param_fw_c_l0,
|
||||
(ParamFloat<px4::params::C_L1>) _param_fw_c_l1,
|
||||
(ParamFloat<px4::params::C_D0>) _param_fw_c_d0,
|
||||
@@ -166,14 +165,21 @@ private:
|
||||
int parameters_update();
|
||||
|
||||
// Update subscriptions
|
||||
void wind_poll();
|
||||
void airspeed_poll();
|
||||
void airflow_aoa_poll();
|
||||
void airflow_slip_poll();
|
||||
|
||||
void vehicle_local_position_poll();
|
||||
void vehicle_attitude_poll();
|
||||
void vehicle_angular_velocity_poll();
|
||||
void vehicle_angular_acceleration_poll();
|
||||
|
||||
void control_update();
|
||||
void manual_control_setpoint_poll();
|
||||
void vehicle_attitude_poll();
|
||||
void vehicle_command_poll();
|
||||
void vehicle_control_mode_poll();
|
||||
void vehicle_status_poll();
|
||||
void wind_poll();
|
||||
|
||||
//
|
||||
void status_publish();
|
||||
@@ -202,19 +208,6 @@ private:
|
||||
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 _pos;
|
||||
Vector3f _pos_sp;
|
||||
Vector3f _vel;
|
||||
Vector3f _vel_sp;
|
||||
Vector3f _acc;
|
||||
Vector3f _acc_sp;
|
||||
Quatf _att;
|
||||
Quatf _att_sp;
|
||||
Vector3f _omega;
|
||||
Vector3f _omega_sp;
|
||||
Vector3f _alpha;
|
||||
*/
|
||||
Vector3f _alpha_sp;
|
||||
Vector3f _wind_estimate;
|
||||
Matrix3f _K_x;
|
||||
@@ -222,6 +215,12 @@ private:
|
||||
Matrix3f _K_a;
|
||||
Matrix3f _K_q;
|
||||
Matrix3f _K_w;
|
||||
Vector3f _pos;
|
||||
Vector3f _vel;
|
||||
Vector3f _acc;
|
||||
Quatf _att; // attitude quaternion
|
||||
Vector3f _omega; // angular rate vector
|
||||
Vector3f _alpha; // angular acceleration vector
|
||||
|
||||
// filter variables
|
||||
std::array<Vector3f, 3> _f_list;
|
||||
@@ -229,6 +228,35 @@ private:
|
||||
std::array<Vector3f, 3> _f_lpf_list;
|
||||
std::array<Vector3f, 3> _a_lpf_list;
|
||||
|
||||
// parameter variables
|
||||
float _mass;
|
||||
float _area;
|
||||
float _rho;
|
||||
float _C_L0;
|
||||
float _C_L1;
|
||||
float _C_D0;
|
||||
float _C_D1;
|
||||
float _C_D2;
|
||||
float _a1;
|
||||
float _a2;
|
||||
float _b1;
|
||||
float _b2;
|
||||
float _b3;
|
||||
|
||||
// misc. variables
|
||||
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.
|
||||
float _airspeed{0.0f};
|
||||
|
||||
bool _aoa_valid{false}; ///< flag if a valid AoA estimate exists
|
||||
hrt_abstime _aoa_last_valid{0}; ///< last time Aoa was received. Used to detect timeouts.
|
||||
float _aoa{0.0f};
|
||||
|
||||
bool _slip_valid{false}; ///< flag if a valid AoA estimate exists
|
||||
hrt_abstime _slip_last_valid{0}; ///< last time Aoa was received. Used to detect timeouts.
|
||||
float _slip{0.0f};
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user