diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index a50cee905d..e92f5dcf1d 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -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[]) { diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp index 71a51f60fd..03c73e7241 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp @@ -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) _param_fw_wing_area, (ParamFloat) _param_rho, // aerodynamic params - (ParamFloat) _param_fw_c_l0, (ParamFloat) _param_fw_c_l1, (ParamFloat) _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 _basis_coeffs_x; // coefficients of the current path Vector _basis_coeffs_y; // coefficients of the current path Vector _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 _f_list; @@ -229,6 +228,35 @@ private: std::array _f_lpf_list; std::array _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}; + + };