yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

fw_att_control: pass time through from run

Browse Source
This commit is contained in:
Daniel Agar
2020-08-07 21:35:39 -04:00
parent 560c22e612
commit 9992bd05db
11 changed files with 59 additions and 106 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
View File

@@ -284,7 +284,10 @@ void FixedwingAttitudeControl::Run()
perf_begin(_loop_perf);
if (_att_sub.update(&_att)) {
// only run controller if attitude changed
vehicle_attitude_s att;
if (_att_sub.update(&att)) {
// only update parameters if they changed
bool params_updated = _parameter_update_sub.updated();
@@ -300,13 +303,11 @@ void FixedwingAttitudeControl::Run()
parameters_update();
}
/* only run controller if attitude changed */
static uint64_t last_run = 0;
float deltaT = constrain((hrt_elapsed_time(&last_run) / 1e6f), 0.01f, 0.1f);
last_run = hrt_absolute_time();
const float dt = math::constrain((att.timestamp - _last_run) * 1e-6f, 0.002f, 0.04f);
_last_run = att.timestamp;
/* get current rotation matrix and euler angles from control state quaternions */
matrix::Dcmf R = matrix::Quatf(_att.q);
matrix::Dcmf R = matrix::Quatf(att.q);
vehicle_angular_velocity_s angular_velocity{};
_vehicle_rates_sub.copy(&angular_velocity);
@@ -379,9 +380,10 @@ void FixedwingAttitudeControl::Run()
wheel_control = true;
}
/* lock integrator until control is started */
/* lock integrator until control is started or for long intervals (> 20 ms) */
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);
|| (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && ! _vehicle_status.in_transition_mode)
|| (dt > 0.02f);
/* if we are in rotary wing mode, do nothing */
if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.is_vtol) {
@@ -389,7 +391,7 @@ void FixedwingAttitudeControl::Run()
return;
}
control_flaps(deltaT);
control_flaps(dt);
/* decide if in stabilized or full manual control */
if (_vcontrol_mode.flag_control_rates_enabled) {
@@ -424,7 +426,7 @@ void FixedwingAttitudeControl::Run()
}
/* Prepare data for attitude controllers */
struct ECL_ControlData control_input = {};
ECL_ControlData control_input{};
control_input.roll = euler_angles.phi();
control_input.pitch = euler_angles.theta();
control_input.yaw = euler_angles.psi();
@@ -502,16 +504,16 @@ void FixedwingAttitudeControl::Run()
/* Run attitude controllers */
if (_vcontrol_mode.flag_control_attitude_enabled) {
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);
_roll_ctrl.control_attitude(dt, control_input);
_pitch_ctrl.control_attitude(dt, control_input);
if (wheel_control) {
_wheel_ctrl.control_attitude(control_input);
_wheel_ctrl.control_attitude(dt, 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);
_yaw_ctrl.control_attitude(dt, control_input);
_wheel_ctrl.reset_integrator();
}
@@ -521,14 +523,14 @@ void FixedwingAttitudeControl::Run()
control_input.yaw_rate_setpoint = _yaw_ctrl.get_desired_rate();
/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
float roll_u = _roll_ctrl.control_euler_rate(control_input);
float roll_u = _roll_ctrl.control_euler_rate(dt, control_input);
_actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll;
if (!PX4_ISFINITE(roll_u)) {
_roll_ctrl.reset_integrator();
}
float pitch_u = _pitch_ctrl.control_euler_rate(control_input);
float pitch_u = _pitch_ctrl.control_euler_rate(dt, control_input);
_actuators.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch : trim_pitch;
if (!PX4_ISFINITE(pitch_u)) {
@@ -538,10 +540,10 @@ void FixedwingAttitudeControl::Run()
float yaw_u = 0.0f;
if (wheel_control) {
yaw_u = _wheel_ctrl.control_bodyrate(control_input);
yaw_u = _wheel_ctrl.control_bodyrate(dt, control_input);
} else {
yaw_u = _yaw_ctrl.control_euler_rate(control_input);
yaw_u = _yaw_ctrl.control_euler_rate(dt, control_input);
}
_actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw;
@@ -597,13 +599,13 @@ void FixedwingAttitudeControl::Run()
_yaw_ctrl.set_bodyrate_setpoint(_rates_sp.yaw);
_pitch_ctrl.set_bodyrate_setpoint(_rates_sp.pitch);
float roll_u = _roll_ctrl.control_bodyrate(control_input);
float roll_u = _roll_ctrl.control_bodyrate(dt, control_input);
_actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll;
float pitch_u = _pitch_ctrl.control_bodyrate(control_input);
float pitch_u = _pitch_ctrl.control_bodyrate(dt, control_input);
_actuators.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch : trim_pitch;
float yaw_u = _yaw_ctrl.control_bodyrate(control_input);
float yaw_u = _yaw_ctrl.control_bodyrate(dt, control_input);
_actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw;
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_rates_sp.thrust_body[0]) ?
@@ -638,7 +640,7 @@ void FixedwingAttitudeControl::Run()
/* lazily publish the setpoint only once available */
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _att.timestamp;
_actuators.timestamp_sample = att.timestamp;
/* Only publish if any of the proper modes are enabled */
if (_vcontrol_mode.flag_control_rates_enabled ||

3
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
View File

@@ -115,7 +115,6 @@ private:
actuator_controls_s _actuators {}; /**< actuator control inputs */
manual_control_setpoint_s _manual_control_setpoint {}; /**< r/c channel data */
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_local_position_s _local_pos {}; /**< local position */
@@ -124,6 +123,8 @@ private:
perf_counter_t _loop_perf; /**< loop performance counter */
hrt_abstime _last_run{0};
float _flaps_applied{0.0f};
float _flaperons_applied{0.0f};

6
src/modules/fw_att_control/ecl_controller.h
View File

@@ -79,9 +79,9 @@ public:
ECL_Controller();
virtual ~ECL_Controller() = default;
virtual float control_attitude(const struct ECL_ControlData &ctl_data) = 0;
virtual float control_euler_rate(const struct ECL_ControlData &ctl_data) = 0;
virtual float control_bodyrate(const struct ECL_ControlData &ctl_data) = 0;
virtual float control_attitude(const float dt, const ECL_ControlData &ctl_data) = 0;
virtual float control_euler_rate(const float dt, const ECL_ControlData &ctl_data) = 0;
virtual float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) = 0;
/* Setters */
void set_time_constant(float time_constant);

23
src/modules/fw_att_control/ecl_pitch_controller.cpp
View File

@@ -43,9 +43,8 @@
#include <lib/ecl/geo/geo.h>
#include <mathlib/mathlib.h>
float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_data)
float ECL_PitchController::control_attitude(const float dt, const ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.pitch_setpoint) &&
PX4_ISFINITE(ctl_data.roll) &&
@@ -64,7 +63,7 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da
return _rate_setpoint;
}
float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_data)
float ECL_PitchController::control_bodyrate(const float dt, const ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.roll) &&
@@ -79,22 +78,10 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da
return math::constrain(_last_output, -1.0f, 1.0f);
}
/* get the usual dt estimate */
uint64_t dt_micros = hrt_elapsed_time(&_last_run);
_last_run = hrt_absolute_time();
float dt = (float)dt_micros * 1e-6f;
/* lock integral for long intervals */
bool lock_integrator = ctl_data.lock_integrator;
if (dt_micros > 500000) {
lock_integrator = true;
}
/* Calculate body angular rate error */
_rate_error = _bodyrate_setpoint - ctl_data.body_y_rate;
if (!lock_integrator && _k_i > 0.0f) {
if (!ctl_data.lock_integrator && _k_i > 0.0f) {
/* Integral term scales with 1/IAS^2 */
float id = _rate_error * dt * ctl_data.scaler * ctl_data.scaler;
@@ -124,7 +111,7 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da
return math::constrain(_last_output, -1.0f, 1.0f);
}
float ECL_PitchController::control_euler_rate(const struct ECL_ControlData &ctl_data)
float ECL_PitchController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data)
{
/* Transform setpoint to body angular rates (jacobian) */
_bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint +
@@ -132,5 +119,5 @@ float ECL_PitchController::control_euler_rate(const struct ECL_ControlData &ctl_
set_bodyrate_setpoint(_bodyrate_setpoint);
return control_bodyrate(ctl_data);
return control_bodyrate(dt, ctl_data);
}

6
src/modules/fw_att_control/ecl_pitch_controller.h
View File

@@ -60,9 +60,9 @@ public:
ECL_PitchController() = default;
~ECL_PitchController() = default;
float control_attitude(const struct ECL_ControlData &ctl_data) override;
float control_euler_rate(const struct ECL_ControlData &ctl_data) override;
float control_bodyrate(const struct ECL_ControlData &ctl_data) override;
float control_attitude(const float dt, const ECL_ControlData &ctl_data) override;
float control_euler_rate(const float dt, const ECL_ControlData &ctl_data) override;
float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) override;
/* Additional Setters */
void set_max_rate_pos(float max_rate_pos)

22
src/modules/fw_att_control/ecl_roll_controller.cpp
View File

@@ -43,7 +43,7 @@
#include <lib/ecl/geo/geo.h>
#include <mathlib/mathlib.h>
float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_data)
float ECL_RollController::control_attitude(const float dt, const ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.roll_setpoint) &&
@@ -61,7 +61,7 @@ float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_dat
return _rate_setpoint;
}
float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_data)
float ECL_RollController::control_bodyrate(const float dt, const ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.pitch) &&
@@ -75,22 +75,10 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat
return math::constrain(_last_output, -1.0f, 1.0f);
}
/* get the usual dt estimate */
uint64_t dt_micros = hrt_elapsed_time(&_last_run);
_last_run = hrt_absolute_time();
float dt = (float)dt_micros * 1e-6f;
/* lock integral for long intervals */
bool lock_integrator = ctl_data.lock_integrator;
if (dt_micros > 500000) {
lock_integrator = true;
}
/* Calculate body angular rate error */
_rate_error = _bodyrate_setpoint - ctl_data.body_x_rate;
if (!lock_integrator && _k_i > 0.0f) {
if (!ctl_data.lock_integrator && _k_i > 0.0f) {
/* Integral term scales with 1/IAS^2 */
float id = _rate_error * dt * ctl_data.scaler * ctl_data.scaler;
@@ -120,12 +108,12 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat
return math::constrain(_last_output, -1.0f, 1.0f);
}
float ECL_RollController::control_euler_rate(const struct ECL_ControlData &ctl_data)
float ECL_RollController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data)
{
/* Transform setpoint to body angular rates (jacobian) */
_bodyrate_setpoint = ctl_data.roll_rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint;
set_bodyrate_setpoint(_bodyrate_setpoint);
return control_bodyrate(ctl_data);
return control_bodyrate(dt, ctl_data);
}

6
src/modules/fw_att_control/ecl_roll_controller.h
View File

@@ -58,9 +58,9 @@ public:
ECL_RollController() = default;
~ECL_RollController() = default;
float control_attitude(const struct ECL_ControlData &ctl_data) override;
float control_euler_rate(const struct ECL_ControlData &ctl_data) override;
float control_bodyrate(const struct ECL_ControlData &ctl_data) override;
float control_attitude(const float dt, const ECL_ControlData &ctl_data) override;
float control_euler_rate(const float dt, const ECL_ControlData &ctl_data) override;
float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) override;
};
#endif // ECL_ROLL_CONTROLLER_H

18
src/modules/fw_att_control/ecl_wheel_controller.cpp
View File

@@ -46,7 +46,7 @@
using matrix::wrap_pi;
float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_data)
float ECL_WheelController::control_bodyrate(const float dt, const ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.body_z_rate) &&
@@ -56,25 +56,13 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da
return math::constrain(_last_output, -1.0f, 1.0f);
}
/* get the usual dt estimate */
uint64_t dt_micros = hrt_elapsed_time(&_last_run);
_last_run = hrt_absolute_time();
float dt = (float)dt_micros * 1e-6f;
/* lock integral for long intervals */
bool lock_integrator = ctl_data.lock_integrator;
if (dt_micros > 500000) {
lock_integrator = true;
}
/* input conditioning */
float min_speed = 1.0f;
/* Calculate body angular rate error */
_rate_error = _rate_setpoint - ctl_data.body_z_rate; //body angular rate error
if (!lock_integrator && _k_i > 0.0f && ctl_data.groundspeed > min_speed) {
if (!ctl_data.lock_integrator && _k_i > 0.0f && ctl_data.groundspeed > min_speed) {
float id = _rate_error * dt * ctl_data.groundspeed_scaler;
@@ -101,7 +89,7 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da
return math::constrain(_last_output, -1.0f, 1.0f);
}
float ECL_WheelController::control_attitude(const struct ECL_ControlData &ctl_data)
float ECL_WheelController::control_attitude(const float dt, const ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) &&

6
src/modules/fw_att_control/ecl_wheel_controller.h
View File

@@ -58,11 +58,11 @@ public:
ECL_WheelController() = default;
~ECL_WheelController() = default;
float control_attitude(const struct ECL_ControlData &ctl_data) override;
float control_attitude(const float dt, const ECL_ControlData &ctl_data) override;
float control_bodyrate(const struct ECL_ControlData &ctl_data) override;
float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) override;
float control_euler_rate(const struct ECL_ControlData &ctl_data) override { (void)ctl_data; return 0; }
float control_euler_rate(const float dt, const ECL_ControlData &ctl_data) override { (void)ctl_data; return 0; }
};
#endif // ECL_HEADING_CONTROLLER_H

23
src/modules/fw_att_control/ecl_yaw_controller.cpp
View File

@@ -43,9 +43,8 @@
#include <lib/ecl/geo/geo.h>
#include <mathlib/mathlib.h>
float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data)
float ECL_YawController::control_attitude(const float dt, const ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.roll) &&
PX4_ISFINITE(ctl_data.pitch) &&
@@ -95,7 +94,7 @@ float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data
return _rate_setpoint;
}
float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data)
float ECL_YawController::control_bodyrate(const float dt, const ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.roll) &&
@@ -110,22 +109,10 @@ float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data
return math::constrain(_last_output, -1.0f, 1.0f);
}
/* get the usual dt estimate */
uint64_t dt_micros = hrt_elapsed_time(&_last_run);
_last_run = hrt_absolute_time();
float dt = (float)dt_micros * 1e-6f;
/* lock integral for long intervals */
bool lock_integrator = ctl_data.lock_integrator;
if (dt_micros > 500000) {
lock_integrator = true;
}
/* Calculate body angular rate error */
_rate_error = _bodyrate_setpoint - ctl_data.body_z_rate;
if (!lock_integrator && _k_i > 0.0f) {
if (!ctl_data.lock_integrator && _k_i > 0.0f) {
/* Integral term scales with 1/IAS^2 */
float id = _rate_error * dt * ctl_data.scaler * ctl_data.scaler;
@@ -155,7 +142,7 @@ float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data
return math::constrain(_last_output, -1.0f, 1.0f);
}
float ECL_YawController::control_euler_rate(const struct ECL_ControlData &ctl_data)
float ECL_YawController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data)
{
/* Transform setpoint to body angular rates (jacobian) */
_bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint +
@@ -163,5 +150,5 @@ float ECL_YawController::control_euler_rate(const struct ECL_ControlData &ctl_da
set_bodyrate_setpoint(_bodyrate_setpoint);
return control_bodyrate(ctl_data);
return control_bodyrate(dt, ctl_data);
}

6
src/modules/fw_att_control/ecl_yaw_controller.h
View File

@@ -58,9 +58,9 @@ public:
ECL_YawController() = default;
~ECL_YawController() = default;
float control_attitude(const struct ECL_ControlData &ctl_data) override;
float control_euler_rate(const struct ECL_ControlData &ctl_data) override;
float control_bodyrate(const struct ECL_ControlData &ctl_data) override;
float control_attitude(const float dt, const ECL_ControlData &ctl_data) override;
float control_euler_rate(const float dt, const ECL_ControlData &ctl_data) override;
float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) override;
protected:
float _max_rate{0.0f};