fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-05 04:51:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

fixed error in position poll fun

Browse Source
This commit is contained in:
Marvin Harms
2022-04-12 09:05:37 +02:00
parent 7eef436c2c
commit 90bcdcf9e1
2 changed files with 25 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
+23 -17
View File
@@ -57,7 +57,7 @@ FixedwingPositionINDIControl::FixedwingPositionINDIControl() :
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
{ {
// limit to 50 Hz // limit to 50 Hz
_local_pos_sub.set_interval_ms(20); _vehicle_local_position_sub.set_interval_ms(20);
/* fetch initial parameter values */ /* fetch initial parameter values */
parameters_update(); parameters_update();
@@ -236,15 +236,22 @@ FixedwingPositionINDIControl::vehicle_angular_acceleration_poll()
void void
FixedwingPositionINDIControl::vehicle_local_position_poll() FixedwingPositionINDIControl::vehicle_local_position_poll()
{ {
vehicle_local_position_s pos; //vehicle_local_position_s pos;
if (_vehicle_local_position_sub.update(&pos)) { if (_vehicle_local_position_sub.update(&_local_pos)) {
_pos = Vector3f(pos.x,pos.y,pos.z); PX4_INFO("updated position!");
_vel = Vector3f(pos.vx,pos.vy,pos.vz); _pos = Vector3f(_local_pos.x,_local_pos.y,_local_pos.z);
_acc = Vector3f(pos.ax,pos.ay,pos.az); _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){ if(hrt_absolute_time()-_local_pos.timestamp > 50_ms){
//PX4_ERR("local position sample is too old"); PX4_ERR("local position sample is too old");
PX4_INFO("timestamps:\t%.1f\t%.1f",(double)hrt_absolute_time(),(double)pos.timestamp); /*
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() FixedwingPositionINDIControl::Run()
{ {
// only run controller if pos, vel, acc changed // only run controller if pos, vel, acc changed
vehicle_local_position_s pos;
perf_begin(_loop_perf); 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 // only update parameters if they changed
bool params_updated = _parameter_update_sub.updated(); 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); //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 // run polls
_set_wind_estimate(Vector3f(0.f,0.f,0.f)); _set_wind_estimate(Vector3f(0.f,0.f,0.f));
@@ -289,9 +294,9 @@ FixedwingPositionINDIControl::Run()
//airflow_slip_poll(); //airflow_slip_poll();
vehicle_local_position_poll(); vehicle_local_position_poll();
//vehicle_attitude_poll(); vehicle_attitude_poll();
//vehicle_angular_velocity_poll(); vehicle_angular_velocity_poll();
//vehicle_angular_acceleration_poll(); vehicle_angular_acceleration_poll();
// compute control input // compute control input
Vector3f ctrl = _compute_NDI_control_input(_pos,_vel,_acc,_att,_omega,_alpha); 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[1] = ctrl(1);
_angular_accel_sp.xyz[2] = ctrl(2); _angular_accel_sp.xyz[2] = ctrl(2);
_alpha_sp_pub.publish(_angular_accel_sp); _alpha_sp_pub.publish(_angular_accel_sp);
//print_message(_angular_accel_sp);
//PX4_INFO("running"); //PX4_INFO("running");
} }
@@ -582,7 +588,7 @@ int FixedwingPositionINDIControl::task_spawn(int argc, char *argv[])
bool bool
FixedwingPositionINDIControl::init() FixedwingPositionINDIControl::init()
{ {
if (!_local_pos_sub.registerCallback()) { if (!_vehicle_local_position_sub.registerCallback()) {
PX4_ERR("vehicle position callback registration failed!"); PX4_ERR("vehicle position callback registration failed!");
return false; return false;
} }
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp
+2 -3
View File
@@ -49,7 +49,6 @@
#include <uORB/topics/vehicle_odometry.h> #include <uORB/topics/vehicle_odometry.h>
#include <uORB/topics/vehicle_acceleration.h> #include <uORB/topics/vehicle_acceleration.h>
#include <uORB/topics/vehicle_land_detected.h> #include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h> #include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_status.h>
#include <uORB/topics/wind.h> #include <uORB/topics/wind.h>
@@ -89,7 +88,7 @@ private:
orb_advert_t _mavlink_log_pub{nullptr}; 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}; 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_aoa_sub{ORB_ID(airflow_aoa)}; // angle of attack
uORB::Subscription _airflow_slip_sub{ORB_ID(airflow_slip)}; // angle of sideslip 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_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_odometry_sub{ORB_ID(vehicle_odometry)}; // vehicle velocity
uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; // vehicle acceleration uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; // vehicle acceleration
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; // vehicle attitude uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; // vehicle attitude