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

created new messages

Browse Source
This commit is contained in:
Marvin Harms
2022-04-19 13:58:30 +02:00
parent 2c9ff19e4f
commit 266d7bc6da
7 changed files with 325 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/rc.fw_apps
+2 -1
View File
@@ -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.
msg/CMakeLists.txt
+2
View File
@@ -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
msg/soaring_controller_heartbeat.msg
+5
View File
@@ -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
msg/soaring_controller_status.msg
+6
View File
@@ -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
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
+183 -50
View File
@@ -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<float, FixedwingPositionINDIControl::_num_basis_funs>
@@ -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;
}
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp
+47 -16
View File
@@ -30,6 +30,7 @@
#include <px4_platform_common/posix.h>
#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/vehicle_command.h>
@@ -41,16 +42,24 @@
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_angular_acceleration.h>
#include <uORB/topics/vehicle_angular_acceleration_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_odometry.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_acceleration.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/rate_ctrl_status.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_status_flags.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/soaring_controller_heartbeat.h>
#include <uORB/topics/soaring_controller_status.h>
#include <uORB/topics/wind.h>
#include <uORB/uORB.h>
#include <iostream>
@@ -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<vehicle_angular_acceleration_setpoint_s> _angular_accel_sp_pub;
uORB::Publication<actuator_controls_s> _actuators_0_pub;
uORB::Publication<vehicle_attitude_setpoint_s> _attitude_sp_pub;
uORB::Publication<vehicle_rates_setpoint_s> _angular_vel_sp_pub{ORB_ID(vehicle_rates_setpoint)};
uORB::Publication<vehicle_angular_acceleration_setpoint_s> _angular_accel_sp_pub{ORB_ID(vehicle_angular_acceleration_setpoint)};
uORB::PublicationMulti<rate_ctrl_status_s> _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)};
uORB::Publication<soaring_controller_heartbeat_s> _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<px4::params::FW_INERTIA_ROLL>) _param_fw_inertia_roll,
(ParamFloat<px4::params::FW_INERTIA_PITCH>) _param_fw_inertia_pitch,
(ParamFloat<px4::params::FW_INERTIA_YAW>) _param_fw_inertia_yaw,
(ParamFloat<px4::params::FW_MASS>) _param_fw_mass,
(ParamFloat<px4::params::FW_WING_AREA>) _param_fw_wing_area,
(ParamFloat<px4::params::RHO>) _param_rho,
@@ -153,7 +174,10 @@ private:
(ParamFloat<px4::params::K_Q_YAW>) _param_k_q_yaw,
(ParamFloat<px4::params::K_W_ROLL>) _param_k_w_roll,
(ParamFloat<px4::params::K_W_PITCH>) _param_k_w_pitch,
(ParamFloat<px4::params::K_W_YAW>) _param_k_w_yaw
(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
)
@@ -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<float, _num_basis_funs> _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<Vector3f, 3> _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
};
src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c
+80
View File
@@ -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);