fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-04 04:30: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);
}