From 266d7bc6da583c52ef5428c316351288a605f35f Mon Sep 17 00:00:00 2001 From: Marvin Harms Date: Tue, 19 Apr 2022 13:58:30 +0200 Subject: [PATCH] created new messages --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 3 +- msg/CMakeLists.txt | 2 + msg/soaring_controller_heartbeat.msg | 5 + msg/soaring_controller_status.msg | 6 + .../FixedwingPositionINDIControl.cpp | 233 ++++++++++++++---- .../FixedwingPositionINDIControl.hpp | 63 +++-- .../fw_dyn_soar_control_params.c | 80 ++++++ 7 files changed, 325 insertions(+), 67 deletions(-) create mode 100644 msg/soaring_controller_heartbeat.msg create mode 100644 msg/soaring_controller_status.msg diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index 95c5a365db..655fa50cff 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -13,8 +13,9 @@ ekf2 start & # # Start attitude controller. # -fw_att_control start +#fw_att_control start fw_pos_control_l1 start +fw_dyn_soar_control start airspeed_selector start # # Start Land Detector. diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 502715c476..86750c9c08 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -140,6 +140,8 @@ set(msg_files sensor_preflight_mag.msg sensor_selection.msg sensors_status_imu.msg + soaring_controller_heartbeat.msg + soaring_controller_status.msg system_power.msg takeoff_status.msg task_stack_info.msg diff --git a/msg/soaring_controller_heartbeat.msg b/msg/soaring_controller_heartbeat.msg new file mode 100644 index 0000000000..1d699c277a --- /dev/null +++ b/msg/soaring_controller_heartbeat.msg @@ -0,0 +1,5 @@ +# SOARING CONTROLLER STATE, SERVES AS A HEARTBEAT IF CONTROLLER IS STILL PUBLISHING + +uint64 timestamp # time since system start (microseconds) + +uint64 heartbeat # time since system start (microseconds) of the last run of soaring controller diff --git a/msg/soaring_controller_status.msg b/msg/soaring_controller_status.msg new file mode 100644 index 0000000000..a9eee91e72 --- /dev/null +++ b/msg/soaring_controller_status.msg @@ -0,0 +1,6 @@ +# SOARING CONTROLLER STATE, SERVES AS FLAG IF CONTROLLER IS STILL PUBLISHING + +uint64 timestamp # time since system start (microseconds) + +bool soaring_controller_running # if true, the soaring controller is expected to publish +bool timeout_detected # if true, the soaring controller has crashed \ No newline at end of file diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index dcada6b12c..6337203937 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -53,11 +53,12 @@ using matrix::wrap_pi; FixedwingPositionINDIControl::FixedwingPositionINDIControl() : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), - _angular_accel_sp_pub(ORB_ID(vehicle_angular_acceleration_setpoint)), + _actuators_0_pub(ORB_ID(actuator_controls_0)), + _attitude_sp_pub(ORB_ID(vehicle_attitude_setpoint)), _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) { // limit to 50 Hz - _vehicle_local_position_sub.set_interval_ms(20); + //_vehicle_angular_velocity_sub.set_interval_ms(20); /* fetch initial parameter values */ parameters_update(); @@ -96,6 +97,9 @@ FixedwingPositionINDIControl::parameters_update() _K_w(2,2) = _param_k_w_yaw.get(); // aircraft parameters + _inertia(0,0) = _param_fw_inertia_roll.get(); + _inertia(1,1) = _param_fw_inertia_pitch.get(); + _inertia(2,2) = _param_fw_inertia_yaw.get(); _mass = _param_fw_mass.get(); _area = _param_fw_wing_area.get(); _rho = _param_rho.get(); @@ -112,6 +116,12 @@ FixedwingPositionINDIControl::parameters_update() _b2 = _param_filter_b2.get(); _b3 = _param_filter_b3.get(); + // actuator gains + _K_actuators *= 0.0f; + _K_actuators(0,0) = _param_k_act_roll.get(); + _K_actuators(1,1) = _param_k_act_pitch.get(); + _K_actuators(2,2) = _param_k_act_yaw.get(); + // sanity check parameters // TODO: include sanity check @@ -201,10 +211,14 @@ void FixedwingPositionINDIControl::vehicle_attitude_poll() { if (_vehicle_attitude_sub.update(&_attitude)) { - _att = Quatf(_attitude.q); + // transform the reference frame from NED to ENU + Dcmf R_in_ned(_attitude.q); + _att = Quatf(_R_ned_to_enu*R_in_ned); + //PX4_INFO("attitude quaternion:\t%.4f\t%.4f\t%.4f\t%.4f", (double)_att(0),(double)_att(1),(double)_att(2),(double)_att(3)); } - if(hrt_absolute_time()-_attitude.timestamp_sample > 50_ms){ + if(hrt_absolute_time()-_attitude.timestamp > 50_ms){ PX4_ERR("attitude sample is too old"); + //PX4_INFO("time differences:\t%.4f\t%.4f", (double)(hrt_absolute_time()-_attitude.timestamp) , (double)(hrt_absolute_time()-_attitude.timestamp_sample)); } } @@ -214,7 +228,7 @@ FixedwingPositionINDIControl::vehicle_angular_velocity_poll() if (_vehicle_angular_velocity_sub.update(&_angular_vel)) { _omega = Vector3f(_angular_vel.xyz); } - if(hrt_absolute_time()-_angular_vel.timestamp_sample > 50_ms){ + if(hrt_absolute_time()-_angular_vel.timestamp > 50_ms){ PX4_ERR("angular velocity sample is too old"); } } @@ -225,7 +239,7 @@ FixedwingPositionINDIControl::vehicle_angular_acceleration_poll() if (_vehicle_angular_acceleration_sub.update(&_angular_accel)) { _alpha = Vector3f(_angular_accel.xyz); } - if(hrt_absolute_time()-_angular_accel.timestamp_sample > 50_ms){ + if(hrt_absolute_time()-_angular_accel.timestamp > 50_ms){ PX4_ERR("angular acceleration sample is too old"); } } @@ -234,21 +248,27 @@ void FixedwingPositionINDIControl::vehicle_local_position_poll() { //vehicle_local_position_s pos; - if (_vehicle_local_position_sub.update(&_local_pos)) { - PX4_INFO("updated position!"); - _pos = Vector3f(_local_pos.x,_local_pos.y,_local_pos.z); - _vel = Vector3f(_local_pos.vx,_local_pos.vy,_local_pos.vz); - _acc = Vector3f(_local_pos.ax,_local_pos.ay,_local_pos.az); + if (_vehicle_local_position_sub.update(&_local_pos)){ + _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); } if(hrt_absolute_time()-_local_pos.timestamp > 50_ms){ PX4_ERR("local position sample is too old"); - /* - PX4_INFO("timestamps:\t%.4f\t%.4f\t%.4f", - (double)hrt_absolute_time(), - (double)_local_pos.timestamp, - (double)_local_pos.timestamp_sample); - */ - //print_message(_local_pos); + //PX4_INFO("time differences:\t%.4f\t%.4f", (double)(hrt_absolute_time()-_local_pos.timestamp) , (double)(hrt_absolute_time()-_local_pos.timestamp_sample)); + } +} + +void +FixedwingPositionINDIControl::soaring_controller_status_poll() +{ + if (_soaring_controller_status_sub.update(&_soaring_controller_status)){ + if (!_soaring_controller_status.soaring_controller_running){ + PX4_INFO("Soaring controller turned off"); + } + if (_soaring_controller_status.timeout_detected){ + PX4_INFO("Controller timeout detected"); + } } } @@ -307,7 +327,7 @@ FixedwingPositionINDIControl::_read_trajectory_coeffs_csv() _basis_coeffs_y(14) = -4286.372935f; _basis_coeffs_y(15) = 679.830536f; - _basis_coeffs_z(0) = 4.971065f; + _basis_coeffs_z(0) = 10.f; //4.971065f; _basis_coeffs_z(1) = -354.548028f; _basis_coeffs_z(2) = 1506.974164f; _basis_coeffs_z(3) = -3506.606108f; @@ -323,7 +343,6 @@ FixedwingPositionINDIControl::_read_trajectory_coeffs_csv() _basis_coeffs_z(13) = -1570.235132f; _basis_coeffs_z(14) = 727.014138f; _basis_coeffs_z(15) = -189.177675; - } void @@ -332,7 +351,7 @@ FixedwingPositionINDIControl::Run() // only run controller if pos, vel, acc changed perf_begin(_loop_perf); - if (_vehicle_local_position_sub.update(&_local_pos)) + if (_vehicle_angular_velocity_sub.update(&_angular_vel)) { // only update parameters if they changed bool params_updated = _parameter_update_sub.updated(); @@ -347,25 +366,57 @@ 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; // run polls _set_wind_estimate(Vector3f(0.f,0.f,0.f)); - //airspeed_poll(); - //airflow_aoa_poll(); - //airflow_slip_poll(); + airspeed_poll(); + airflow_aoa_poll(); + airflow_slip_poll(); vehicle_local_position_poll(); vehicle_attitude_poll(); vehicle_angular_velocity_poll(); vehicle_angular_acceleration_poll(); + soaring_controller_status_poll(); + // ============================ + // compute reference kinematics + // ============================ + /* + // get reference values + float t_ref = _get_closest_t(_pos); + // 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.001f)); + Vector3f pos_ref = _get_position_ref(t_ref); // in inertial ENU + Vector3f vel_ref = _get_velocity_ref(t_ref,T); // in inertial ENU + Vector3f acc_ref = _get_acceleration_ref(t_ref,T); // gravity-corrected acceleration (ENU) + Quatf q = _get_attitude_ref(t_ref,T); + Vector3f omega_ref = _get_angular_velocity_ref(t_ref,T); // body angular velocity + Vector3f alpha_ref = _get_angular_acceleration_ref(t_ref,T); // body angular acceleration + */ + + //TODO: remove when done with testing + Vector3f pos_ref = _pos + Vector3f{1.f,0.f,1.f}; // in inertial ENU + Vector3f vel_ref = Vector3f{15.f,0.f,0.f}; // in inertial ENU + Vector3f acc_ref = Vector3f{0.f,0.f,0.f}; // gravity-corrected acceleration (ENU) + Quatf q = _get_attitude_ref(0.f,10.f); + Vector3f omega_ref = Vector3f{0.f,0.f,0.f}; // body angular velocity + Vector3f alpha_ref = Vector3f{0.f,0.f,0.f}; // body angular acceleration + + + + // ===================== // compute control input - Vector3f ctrl = _compute_NDI_control_input(_pos,_vel,_acc,_att,_omega,_alpha); + // ===================== + Vector3f ctrl = _compute_NDI_stage_1(pos_ref, vel_ref, acc_ref, omega_ref, alpha_ref); + // ===================== // publish control input + // ===================== //_angular_accel_sp = {}; _angular_accel_sp.timestamp = hrt_absolute_time(); _angular_accel_sp.timestamp_sample = hrt_absolute_time(); @@ -374,16 +425,78 @@ FixedwingPositionINDIControl::Run() _angular_accel_sp.xyz[2] = ctrl(2); _angular_accel_sp_pub.publish(_angular_accel_sp); //print_message(_angular_accel_sp); - /* - Quatf vec = _get_attitude_ref(0.2,1); - + + // ========================= + // publish attitude setpoint + // ========================= + _attitude_sp = {}; + _attitude_sp.timestamp = hrt_absolute_time(); + // transform quaternion back to NED frame + Dcmf R(q); + Quatf q_transformed(_R_enu_to_ned*R); + _attitude_sp.q_d[0] = q_transformed(0); + _attitude_sp.q_d[1] = q_transformed(1); + _attitude_sp.q_d[2] = q_transformed(2); + _attitude_sp.q_d[3] = q_transformed(3); + _attitude_sp_pub.publish(_attitude_sp); + + // ====================== + // publish rates setpoint + // ====================== + _angular_vel_sp = {}; + _angular_vel_sp.timestamp = hrt_absolute_time(); + _angular_vel_sp.roll = omega_ref(0); + _angular_vel_sp.pitch = omega_ref(1); + _angular_vel_sp.yaw = omega_ref(2); + _angular_vel_sp_pub.publish(_angular_vel_sp); + + // ============================ + // compute actuator deflections + // ============================ + Vector3f ctrl1 = _compute_NDI_stage_2(ctrl); + Vector3f ctrl2 = _compute_actuator_deflections(ctrl1); + + // ========================= + // publish acutator controls + // ========================= + _actuators = {}; + _actuators.timestamp = hrt_absolute_time(); + _actuators.timestamp_sample = hrt_absolute_time(); + _actuators.control[actuator_controls_s::INDEX_ROLL] = ctrl2(0); + _actuators.control[actuator_controls_s::INDEX_PITCH] = ctrl2(1); + _actuators.control[actuator_controls_s::INDEX_YAW] = ctrl2(2); + _actuators.control[actuator_controls_s::INDEX_THROTTLE] = 1.0f; + _actuators_0_pub.publish(_actuators); + //print_message(_actuators); + + // =========================== + // publish rate control status + // =========================== + rate_ctrl_status_s rate_ctrl_status{}; + rate_ctrl_status.timestamp = hrt_absolute_time(); + rate_ctrl_status.rollspeed_integ = 0.0f; + rate_ctrl_status.pitchspeed_integ = 0.0f; + rate_ctrl_status.yawspeed_integ = 0.0f; + _rate_ctrl_status_pub.publish(rate_ctrl_status); + + /* PX4_INFO("control setpoints:\t%.4f\t%.4f\t%.4f", (double)vec(0), (double)vec(1), (double)vec(2)); */ + + // ============================== + // publish soaring control status + // ============================== + //_soaring_controller_heartbeat_s _soaring_controller_heartbeat{}; + _soaring_controller_heartbeat.timestamp = hrt_absolute_time(); + _soaring_controller_heartbeat.heartbeat = hrt_absolute_time(); + _soaring_controller_heartbeat_pub.publish(_soaring_controller_heartbeat); + //print_message(_soaring_controller_heartbeat); } + perf_end(_loop_perf); } Vector @@ -555,6 +668,7 @@ FixedwingPositionINDIControl::_get_closest_t(Vector3f pos) t = float(i)/n; } } + //PX4_INFO("closest distance:%.2f", (double)min_dist); return t; } @@ -597,44 +711,56 @@ FixedwingPositionINDIControl::_get_attitude(Vector3f vel, Vector3f f) } Vector3f -FixedwingPositionINDIControl::_compute_NDI_control_input(Vector3f pos, Vector3f vel, Vector3f acc, Quatf att, Vector3f omega, Vector3f alpha) +FixedwingPositionINDIControl::_compute_NDI_stage_1(Vector3f pos_ref, Vector3f vel_ref, Vector3f acc_ref, Vector3f omega_ref, Vector3f alpha_ref) { - Dcmf R_ib(att); + Dcmf R_ib(_att); 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.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); - Vector3f a_ref = _get_acceleration_ref(t_ref,T); - Vector3f omega_ref = _get_angular_velocity_ref(t_ref,T); - Vector3f alpha_ref = _get_angular_acceleration_ref(t_ref,T); // get acceleration command in world frame (without gravity) - Vector3f acc_command = R_ib*(_K_x*R_bi*(x_ref-pos) + _K_v*R_bi*(v_ref-vel) + _K_a*R_bi*(a_ref-acc)) + a_ref; + Vector3f acc_command = R_ib*(_K_x*R_bi*(pos_ref-_pos) + _K_v*R_bi*(vel_ref-_vel) + _K_a*R_bi*(acc_ref-_acc)) + acc_ref; // add gravity acc_command(2) += 9.81f; // get force comand in world frame 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)); + Dcmf R_ref(_get_attitude(vel_ref,f_command)); // get attitude error Dcmf R_ref_true(R_ref.transpose()*R_ib); // get required rotation vector (in body frame) AxisAnglef q_err(R_ref_true); Vector3f w_err = -q_err.angle()*q_err.axis(); - //Vector3f v_norm = (vel-_wind_estimate).unit(); - //Vector3f v_ref_norm = (v_ref-_wind_estimate).unit(); // compute angular acceleration command (in body frame) - Vector3f rot_acc_command = _K_q*w_err + _K_w*(omega_ref-omega) + alpha_ref; + Vector3f rot_acc_command = _K_q*w_err + _K_w*(omega_ref-_omega) + alpha_ref; return rot_acc_command; } +Vector3f +FixedwingPositionINDIControl::_compute_NDI_stage_2(Vector3f ctrl) +{ + // compute the required body moment to produce the desired body angular acceleration + Vector3f moment = _inertia*ctrl + _omega.cross(_inertia*_omega); + return moment; +} + +Vector3f +FixedwingPositionINDIControl::_compute_actuator_deflections(Vector3f ctrl) +{ + // compute airspeed scaling + const float airspeed_constrained = constrain(_airspeed, 5.f, 50.f); + float airspeed_scaling = 1.f/(powf(airspeed_constrained,2)+1.f); + + // compute the normalized actuator deflection, including airspeed scaling + Vector3f deflection = airspeed_scaling*_K_actuators*ctrl; + + // limit actuator deflection + for(int i=0; i<3; i++){ + deflection(i) = constrain(deflection(i),-1.f,1.f); + } + return deflection; +} + + + int FixedwingPositionINDIControl::task_spawn(int argc, char *argv[]) { FixedwingPositionINDIControl *instance = new FixedwingPositionINDIControl(); @@ -661,11 +787,18 @@ int FixedwingPositionINDIControl::task_spawn(int argc, char *argv[]) bool FixedwingPositionINDIControl::init() { - if (!_vehicle_local_position_sub.registerCallback()) { + if (!_vehicle_angular_velocity_sub.registerCallback()) { PX4_ERR("vehicle position callback registration failed!"); return false; } _read_trajectory_coeffs_csv(); + // initialize transformations + _R_ned_to_enu *= 0.f; + _R_ned_to_enu(0,1) = 1; + _R_ned_to_enu(1,0) = 1; + _R_ned_to_enu(2,2) = -1; + _R_ned_to_enu.renormalize(); + _R_enu_to_ned = _R_ned_to_enu; return true; } diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp index 009210bfe1..879e91d489 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp @@ -30,6 +30,7 @@ #include #include #include +#include #include #include #include @@ -41,16 +42,24 @@ #include #include #include +#include #include +#include #include #include #include #include #include +#include #include #include #include +#include #include +#include +#include +#include +#include #include #include #include @@ -90,7 +99,8 @@ private: orb_advert_t _mavlink_log_pub{nullptr}; - uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)}; + // make the main task run, whenever a new body rate becomes available + uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -98,31 +108,42 @@ 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_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 + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; // local NED position + uORB::Subscription _home_position_sub{ORB_ID(home_position)}; // home position uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; // vehicle attitude - uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; // vehicle body rates uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)}; // vehicle body accel + uORB::Subscription _soaring_controller_status_sub{ORB_ID(soaring_controller_status)}; // vehicle status flags - // Publishers - uORB::Publication _angular_accel_sp_pub; - + uORB::Publication _actuators_0_pub; + uORB::Publication _attitude_sp_pub; + uORB::Publication _angular_vel_sp_pub{ORB_ID(vehicle_rates_setpoint)}; + uORB::Publication _angular_accel_sp_pub{ORB_ID(vehicle_angular_acceleration_setpoint)}; + uORB::PublicationMulti _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)}; + uORB::Publication _soaring_controller_heartbeat_pub{ORB_ID(soaring_controller_status)}; + // Message structs vehicle_angular_acceleration_setpoint_s _angular_accel_sp {}; + actuator_controls_s _actuators {}; // actuator commands manual_control_setpoint_s _manual_control_setpoint {}; ///< r/c channel data vehicle_local_position_s _local_pos {}; ///< vehicle local position vehicle_attitude_s _attitude {}; ///< vehicle attitude + vehicle_attitude_setpoint_s _attitude_sp {}; ///< vehicle attitude setpoint vehicle_angular_velocity_s _angular_vel {}; ///< vehicle angular velocity + 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 vehicle_control_mode_s _control_mode {}; ///< control mode vehicle_status_s _vehicle_status {}; ///< vehicle status + soaring_controller_status_s _soaring_controller_status {}; ///< soaring controller status + soaring_controller_heartbeat_s _soaring_controller_heartbeat{}; ///< soaring controller hrt // parameter struct DEFINE_PARAMETERS( // aircraft params + (ParamFloat) _param_fw_inertia_roll, + (ParamFloat) _param_fw_inertia_pitch, + (ParamFloat) _param_fw_inertia_yaw, (ParamFloat) _param_fw_mass, (ParamFloat) _param_fw_wing_area, (ParamFloat) _param_rho, @@ -153,7 +174,10 @@ private: (ParamFloat) _param_k_q_yaw, (ParamFloat) _param_k_w_roll, (ParamFloat) _param_k_w_pitch, - (ParamFloat) _param_k_w_yaw + (ParamFloat) _param_k_w_yaw, + (ParamFloat) _param_k_act_roll, + (ParamFloat) _param_k_act_pitch, + (ParamFloat) _param_k_act_yaw ) @@ -184,6 +208,7 @@ private: void vehicle_command_poll(); void vehicle_control_mode_poll(); void vehicle_status_poll(); + void soaring_controller_status_poll(); // void status_publish(); @@ -206,8 +231,9 @@ private: Vector3f _get_angular_velocity_ref(float t=0, float T=1); // get the reference angular velocity on the current path, at normalized time t in [0,1], with an intended cycle time of T Vector3f _get_angular_acceleration_ref(float t=0, float T=1); // get the reference angular acceleration on the current path, at normalized time t in [0,1], with an intended cycle time of T Quatf _get_attitude(Vector3f vel, Vector3f f); // get the attitude to produce force f while flying with velocity vel - Vector3f _compute_NDI_control_input(Vector3f pos, Vector3f vel, Vector3f acc, Quatf att, Vector3f omega, Vector3f alpha); - Vector3f _compute_INDI_control_input(Vector3f pos, Vector3f vel, Vector3f acc, Quatf att, Vector3f omega, Vector3f alpha); + Vector3f _compute_NDI_stage_1(Vector3f pos_ref, Vector3f vel_ref, Vector3f acc_ref, Vector3f omega_ref, Vector3f alpha_ref); + Vector3f _compute_NDI_stage_2(Vector3f ctrl); + Vector3f _compute_actuator_deflections(Vector3f ctrl); // control variables Vector _basis_coeffs_x = {}; // coefficients of the current path @@ -220,12 +246,13 @@ private: Matrix3f _K_a; Matrix3f _K_q; Matrix3f _K_w; - Vector3f _pos; - Vector3f _vel; - Vector3f _acc; + Vector3f _pos; // current position + Vector3f _vel; // current velocity + Vector3f _acc; // current acceleration Quatf _att; // attitude quaternion Vector3f _omega; // angular rate vector Vector3f _alpha; // angular acceleration vector + Matrix3f _K_actuators; // diagonal actuator control gain matrix hrt_abstime _last_run{0}; // filter variables @@ -235,6 +262,7 @@ private: std::array _a_lpf_list; // parameter variables + Matrix3f _inertia {}; float _mass; float _area; float _rho; @@ -249,7 +277,6 @@ private: 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}; @@ -262,6 +289,10 @@ private: hrt_abstime _slip_last_valid{0}; ///< last time Aoa was received. Used to detect timeouts. float _slip{0.0f}; + // helper variables + Dcmf _R_ned_to_enu; // rotation matrix from NED to ENU frame + Dcmf _R_enu_to_ned; // rotation matrix from ENU to NED frame + Vector3f _zero_crossing_local_pos; // vector denoting the zero crossing of the trajectories in NED frame }; 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 f10042c037..1b9cb4ad41 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 @@ -10,6 +10,50 @@ * Controller parameters, accessible via MAVLink */ +/** + * inertia around body x-axis + * + * This is the inertia of the aircraft, used for the INDI + * + * @unit kg + * @min 0.01 + * @max 10 + * @decimal 2 + * @increment 0.01 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(FW_INERTIA_ROLL, 1.0f); + +/** + * inertia around body y-axis + * + * This is the inertia of the aircraft, used for the INDI + * + * @unit kg + * @min 0.01 + * @max 10 + * @decimal 2 + * @increment 0.01 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(FW_INERTIA_PITCH, 1.0f); + +/** + * inertia around body z-axis + * + * This is the inertia of the aircraft, used for the INDI + * + * @unit kg + * @min 0.01 + * @max 10 + * @decimal 2 + * @increment 0.01 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(FW_INERTIA_YAW, 1.0f); + + + /** * total takeoff mass * @@ -368,3 +412,39 @@ PARAM_DEFINE_FLOAT(K_W_PITCH, 5.0f); * @group FW DYN SOAR Control */ PARAM_DEFINE_FLOAT(K_W_YAW, 5.0f); + +/** + * roll gain of K_ACT (actuator deflection gain) + * + * @unit + * @min 0 + * @max 100 + * @decimal 1 + * @increment 0.1 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(K_ACT_ROLL, 3.0f); + +/** + * pitch gain of K_ACT (actuator deflection gain) + * + * @unit + * @min 0 + * @max 100 + * @decimal 1 + * @increment 0.1 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(K_ACT_PITCH, 3.0f); + +/** + * yaw gain of K_ACT (actuator deflection gain) + * + * @unit + * @min 0 + * @max 100 + * @decimal 1 + * @increment 0.1 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(K_ACT_YAW, 3.0f);