From 90bcdcf9e1d436b3bdd92afbfcb924a9252edabc Mon Sep 17 00:00:00 2001 From: Marvin Harms Date: Tue, 12 Apr 2022 09:05:37 +0200 Subject: [PATCH] fixed error in position poll fun --- .../FixedwingPositionINDIControl.cpp | 40 +++++++++++-------- .../FixedwingPositionINDIControl.hpp | 5 +-- 2 files changed, 25 insertions(+), 20 deletions(-) diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index 0bb56f260e..fa47a56b6c 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -57,7 +57,7 @@ FixedwingPositionINDIControl::FixedwingPositionINDIControl() : _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) { // limit to 50 Hz - _local_pos_sub.set_interval_ms(20); + _vehicle_local_position_sub.set_interval_ms(20); /* fetch initial parameter values */ parameters_update(); @@ -236,15 +236,22 @@ FixedwingPositionINDIControl::vehicle_angular_acceleration_poll() 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); + //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(hrt_absolute_time()-pos.timestamp > 50_ms){ - //PX4_ERR("local position sample is too old"); - PX4_INFO("timestamps:\t%.1f\t%.1f",(double)hrt_absolute_time(),(double)pos.timestamp); + 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); } } @@ -259,11 +266,9 @@ void FixedwingPositionINDIControl::Run() { // only run controller if pos, vel, acc changed - vehicle_local_position_s pos; - perf_begin(_loop_perf); - if (_vehicle_local_position_sub.update(&pos)) + if (_vehicle_local_position_sub.update(&_local_pos)) { // only update parameters if they changed bool params_updated = _parameter_update_sub.updated(); @@ -280,7 +285,7 @@ FixedwingPositionINDIControl::Run() } //const float dt = math::constrain((pos.timestamp - _last_run) * 1e-6f, 0.002f, 0.04f); - _last_run = pos.timestamp; + _last_run = _local_pos.timestamp; // run polls _set_wind_estimate(Vector3f(0.f,0.f,0.f)); @@ -289,9 +294,9 @@ FixedwingPositionINDIControl::Run() //airflow_slip_poll(); vehicle_local_position_poll(); - //vehicle_attitude_poll(); - //vehicle_angular_velocity_poll(); - //vehicle_angular_acceleration_poll(); + vehicle_attitude_poll(); + vehicle_angular_velocity_poll(); + vehicle_angular_acceleration_poll(); // compute control input Vector3f ctrl = _compute_NDI_control_input(_pos,_vel,_acc,_att,_omega,_alpha); @@ -303,6 +308,7 @@ FixedwingPositionINDIControl::Run() _angular_accel_sp.xyz[1] = ctrl(1); _angular_accel_sp.xyz[2] = ctrl(2); _alpha_sp_pub.publish(_angular_accel_sp); + //print_message(_angular_accel_sp); //PX4_INFO("running"); } @@ -582,7 +588,7 @@ int FixedwingPositionINDIControl::task_spawn(int argc, char *argv[]) bool FixedwingPositionINDIControl::init() { - if (!_local_pos_sub.registerCallback()) { + if (!_vehicle_local_position_sub.registerCallback()) { PX4_ERR("vehicle position callback registration failed!"); return false; } diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp index 79a6d54c6f..f3d75f5d98 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp @@ -49,7 +49,6 @@ #include #include #include -#include #include #include #include @@ -89,7 +88,7 @@ private: orb_advert_t _mavlink_log_pub{nullptr}; - uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)}; + uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -98,7 +97,7 @@ private: 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_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_attitude_sub{ORB_ID(vehicle_attitude)}; // vehicle attitude