mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-13 10:53:37 +08:00
created new messages
This commit is contained in:
Marvin Harms
@@ -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.
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
};
|
||||
|
||||
|
||||
@@ -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);
|
||||
|
||||
Reference in New Issue
Block a user