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

vehicle_global_position: remove velocity fields (duplicates of local vx, vy, vz)

Browse Source 
* attitude_estimator_q: get velocity from local position (if available)
This commit is contained in:
Daniel Agar
2020-03-11 23:57:43 -04:00
committed by GitHub
parent 5d33b9e999
commit a89b69b0ea
19 changed files with 98 additions and 79 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+40 -17
View File
@@ -361,13 +361,19 @@ void FixedwingAttitudeControl::Run()
vehicle_control_mode_poll();
vehicle_manual_poll();
_global_pos_sub.update(&_global_pos);
vehicle_land_detected_poll();
// the position controller will not emit attitude setpoints in some modes
// we need to make sure that this flag is reset
_att_sp.fw_control_yaw = _att_sp.fw_control_yaw && _vcontrol_mode.flag_control_auto_enabled;
bool wheel_control = false;
// TODO: manual wheel_control on ground?
if (_param_fw_w_en.get() && _att_sp.fw_control_yaw) {
wheel_control = true;
}
/* lock integrator until control is started */
bool lock_integrator = !_vcontrol_mode.flag_control_rates_enabled
|| (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && ! _vehicle_status.in_transition_mode);
@@ -393,14 +399,6 @@ void FixedwingAttitudeControl::Run()
const float airspeed = get_airspeed_and_update_scaling();
/* Use min airspeed to calculate ground speed scaling region.
* Don't scale below gspd_scaling_trim
*/
float groundspeed = sqrtf(_global_pos.vel_n * _global_pos.vel_n +
_global_pos.vel_e * _global_pos.vel_e);
float gspd_scaling_trim = (_param_fw_airspd_min.get() * 0.6f);
float groundspeed_scaler = gspd_scaling_trim / ((groundspeed < gspd_scaling_trim) ? gspd_scaling_trim : groundspeed);
/* reset integrals where needed */
if (_att_sp.roll_reset_integral) {
_roll_ctrl.reset_integrator();
@@ -444,8 +442,20 @@ void FixedwingAttitudeControl::Run()
control_input.airspeed = airspeed;
control_input.scaler = _airspeed_scaling;
control_input.lock_integrator = lock_integrator;
control_input.groundspeed = groundspeed;
control_input.groundspeed_scaler = groundspeed_scaler;
if (wheel_control) {
_local_pos_sub.update(&_local_pos);
/* Use min airspeed to calculate ground speed scaling region.
* Don't scale below gspd_scaling_trim
*/
float groundspeed = sqrtf(_local_pos.vx * _local_pos.vx + _local_pos.vy * _local_pos.vy);
float gspd_scaling_trim = (_param_fw_airspd_min.get() * 0.6f);
float groundspeed_scaler = gspd_scaling_trim / ((groundspeed < gspd_scaling_trim) ? gspd_scaling_trim : groundspeed);
control_input.groundspeed = groundspeed;
control_input.groundspeed_scaler = groundspeed_scaler;
}
/* reset body angular rate limits on mode change */
if ((_vcontrol_mode.flag_control_attitude_enabled != _flag_control_attitude_enabled_last) || params_updated) {
@@ -497,8 +507,16 @@ void FixedwingAttitudeControl::Run()
if (PX4_ISFINITE(_att_sp.roll_body) && PX4_ISFINITE(_att_sp.pitch_body)) {
_roll_ctrl.control_attitude(control_input);
_pitch_ctrl.control_attitude(control_input);
_yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude
_wheel_ctrl.control_attitude(control_input);
if (wheel_control) {
_wheel_ctrl.control_attitude(control_input);
_yaw_ctrl.reset_integrator();
} else {
// runs last, because is depending on output of roll and pitch attitude
_yaw_ctrl.control_attitude(control_input);
_wheel_ctrl.reset_integrator();
}
/* Update input data for rate controllers */
control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate();
@@ -522,7 +540,7 @@ void FixedwingAttitudeControl::Run()
float yaw_u = 0.0f;
if (_param_fw_w_en.get() && _att_sp.fw_control_yaw) {
if (wheel_control) {
yaw_u = _wheel_ctrl.control_bodyrate(control_input);
} else {
@@ -595,12 +613,17 @@ void FixedwingAttitudeControl::Run()
_rates_sp.thrust_body[0] : 0.0f;
}
rate_ctrl_status_s rate_ctrl_status;
rate_ctrl_status_s rate_ctrl_status{};
rate_ctrl_status.timestamp = hrt_absolute_time();
rate_ctrl_status.rollspeed_integ = _roll_ctrl.get_integrator();
rate_ctrl_status.pitchspeed_integ = _pitch_ctrl.get_integrator();
rate_ctrl_status.yawspeed_integ = _yaw_ctrl.get_integrator();
rate_ctrl_status.additional_integ1 = _wheel_ctrl.get_integrator();
if (wheel_control) {
rate_ctrl_status.additional_integ1 = _wheel_ctrl.get_integrator();
} else {
rate_ctrl_status.yawspeed_integ = _yaw_ctrl.get_integrator();
}
_rate_ctrl_status_pub.publish(rate_ctrl_status);
}
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
+3 -3
View File
@@ -62,7 +62,7 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_status.h>
@@ -97,7 +97,7 @@ private:
uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; /**< vehicle attitude setpoint */
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; /**< battery status subscription */
uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)}; /**< global position subscription */
uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; /**< local position subscription */
uORB::Subscription _manual_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */
uORB::Subscription _rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint */
@@ -120,7 +120,7 @@ private:
vehicle_attitude_s _att {}; /**< vehicle attitude setpoint */
vehicle_attitude_setpoint_s _att_sp {}; /**< vehicle attitude setpoint */
vehicle_control_mode_s _vcontrol_mode {}; /**< vehicle control mode */
vehicle_global_position_s _global_pos {}; /**< global position */
vehicle_local_position_s _local_pos {}; /**< local position */
vehicle_rates_setpoint_s _rates_sp {}; /* attitude rates setpoint */
vehicle_status_s _vehicle_status {}; /**< vehicle status */