fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-14 13:50:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

fw_att_control: move to WQ with uORB callback scheduling

Browse Source
This commit is contained in:
Daniel Agar
2019-08-07 22:54:44 -04:00
committed by GitHub
parent 0955fd2d58
commit cab0aee2a0
2 changed files with 375 additions and 403 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+365 -392
View File
@@ -45,10 +45,9 @@ using namespace time_literals;
extern "C" __EXPORT int fw_att_control_main(int argc, char *argv[]);
FixedwingAttitudeControl::FixedwingAttitudeControl() :
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "fwa_dt")),
_nonfinite_input_perf(perf_alloc(PC_COUNT, "fwa_nani")),
_nonfinite_output_perf(perf_alloc(PC_COUNT, "fwa_nano"))
WorkItem(px4::wq_configurations::att_pos_ctrl),
_loop_perf(perf_alloc(PC_ELAPSED, "fw_att_control: cycle")),
_loop_interval_perf(perf_alloc(PC_INTERVAL, "fw_att_control: interval"))
{
// check if VTOL first
vehicle_status_poll();
@@ -128,18 +127,23 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_pitch_ctrl.set_max_rate_pos(_parameters.acro_max_y_rate_rad);
_pitch_ctrl.set_max_rate_neg(_parameters.acro_max_y_rate_rad);
_yaw_ctrl.set_max_rate(_parameters.acro_max_z_rate_rad);
// subscriptions
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
}
FixedwingAttitudeControl::~FixedwingAttitudeControl()
{
orb_unsubscribe(_att_sub);
perf_free(_loop_perf);
perf_free(_nonfinite_input_perf);
perf_free(_nonfinite_output_perf);
perf_free(_loop_interval_perf);
}
bool
FixedwingAttitudeControl::init()
{
if (!_att_sub.register_callback()) {
PX4_ERR("vehicle attitude callback registration failed!");
return false;
}
return true;
}
int
@@ -280,6 +284,7 @@ FixedwingAttitudeControl::vehicle_manual_poll()
const bool is_fixed_wing = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
if (_vcontrol_mode.flag_control_manual_enabled && (!is_tailsitter_transition || is_fixed_wing)) {
// Always copy the new manual setpoint, even if it wasn't updated, to fill the _actuators with valid values
if (_manual_sub.copy(&_manual)) {
@@ -413,10 +418,6 @@ float FixedwingAttitudeControl::get_airspeed_and_update_scaling()
&& (hrt_elapsed_time(&_airspeed_sub.get().timestamp) < 1_s)
&& !_vehicle_status.aspd_use_inhibit;
if (!airspeed_valid) {
perf_count(_nonfinite_input_perf);
}
// if no airspeed measurement is available out best guess is to use the trim airspeed
float airspeed = _parameters.airspeed_trim;
@@ -444,20 +445,21 @@ float FixedwingAttitudeControl::get_airspeed_and_update_scaling()
const float airspeed_constrained = math::constrain(airspeed, _parameters.airspeed_min, _parameters.airspeed_max);
_airspeed_scaling = _parameters.airspeed_trim / airspeed_constrained;
return airspeed;
}
void FixedwingAttitudeControl::run()
void FixedwingAttitudeControl::Run()
{
/* wakeup source */
px4_pollfd_struct_t fds[1];
if (should_exit()) {
_att_sub.unregister_callback();
exit_and_cleanup();
return;
}
/* Setup of loop */
fds[0].fd = _att_sub;
fds[0].events = POLLIN;
perf_begin(_loop_perf);
perf_count(_loop_interval_perf);
while (!should_exit()) {
if (_att_sub.update(&_att)) {
/* only update parameters if they changed */
bool params_updated = _params_sub.updated();
@@ -471,393 +473,362 @@ void FixedwingAttitudeControl::run()
parameters_update();
}
/* wait for up to 500ms for data */
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit, etc. */
if (pret == 0) {
continue;
}
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
PX4_WARN("poll error %d, %d", pret, errno);
continue;
}
perf_begin(_loop_perf);
/* only run controller if attitude changed */
if (fds[0].revents & POLLIN) {
static uint64_t last_run = 0;
float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
static uint64_t last_run = 0;
float deltaT = math::constrain((hrt_elapsed_time(&last_run) / 1e6f), 0.01f, 0.1f);
last_run = hrt_absolute_time();
/* guard against too large deltaT's */
if (deltaT > 1.0f) {
deltaT = 0.01f;
/* get current rotation matrix and euler angles from control state quaternions */
matrix::Dcmf R = matrix::Quatf(_att.q);
vehicle_angular_velocity_s angular_velocity{};
_vehicle_rates_sub.copy(&angular_velocity);
float rollspeed = angular_velocity.xyz[0];
float pitchspeed = angular_velocity.xyz[1];
float yawspeed = angular_velocity.xyz[2];
if (_is_tailsitter) {
/* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode
*
* Since the VTOL airframe is initialized as a multicopter we need to
* modify the estimated attitude for the fixed wing operation.
* Since the neutral position of the vehicle in fixed wing mode is -90 degrees rotated around
* the pitch axis compared to the neutral position of the vehicle in multicopter mode
* we need to swap the roll and the yaw axis (1st and 3rd column) in the rotation matrix.
* Additionally, in order to get the correct sign of the pitch, we need to multiply
* the new x axis of the rotation matrix with -1
*
* original: modified:
*
* Rxx Ryx Rzx -Rzx Ryx Rxx
* Rxy Ryy Rzy -Rzy Ryy Rxy
* Rxz Ryz Rzz -Rzz Ryz Rxz
* */
matrix::Dcmf R_adapted = R; //modified rotation matrix
/* move z to x */
R_adapted(0, 0) = R(0, 2);
R_adapted(1, 0) = R(1, 2);
R_adapted(2, 0) = R(2, 2);
/* move x to z */
R_adapted(0, 2) = R(0, 0);
R_adapted(1, 2) = R(1, 0);
R_adapted(2, 2) = R(2, 0);
/* change direction of pitch (convert to right handed system) */
R_adapted(0, 0) = -R_adapted(0, 0);
R_adapted(1, 0) = -R_adapted(1, 0);
R_adapted(2, 0) = -R_adapted(2, 0);
/* fill in new attitude data */
R = R_adapted;
/* lastly, roll- and yawspeed have to be swaped */
float helper = rollspeed;
rollspeed = -yawspeed;
yawspeed = helper;
}
const matrix::Eulerf euler_angles(R);
vehicle_attitude_setpoint_poll();
vehicle_control_mode_poll();
vehicle_manual_poll();
_global_pos_sub.update(&_global_pos);
vehicle_status_poll();
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;
/* 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);
/* Simple handling of failsafe: deploy parachute if failsafe is on */
if (_vcontrol_mode.flag_control_termination_enabled) {
_actuators_airframe.control[7] = 1.0f;
} else {
_actuators_airframe.control[7] = 0.0f;
}
/* 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) {
perf_end(_loop_perf);
return;
}
control_flaps(deltaT);
/* decide if in stabilized or full manual control */
if (_vcontrol_mode.flag_control_rates_enabled) {
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 = (_parameters.airspeed_min * 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();
}
/* load local copies */
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
/* get current rotation matrix and euler angles from control state quaternions */
matrix::Dcmf R = matrix::Quatf(_att.q);
vehicle_angular_velocity_s angular_velocity{};
_vehicle_rates_sub.copy(&angular_velocity);
float rollspeed = angular_velocity.xyz[0];
float pitchspeed = angular_velocity.xyz[1];
float yawspeed = angular_velocity.xyz[2];
if (_is_tailsitter) {
/* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode
*
* Since the VTOL airframe is initialized as a multicopter we need to
* modify the estimated attitude for the fixed wing operation.
* Since the neutral position of the vehicle in fixed wing mode is -90 degrees rotated around
* the pitch axis compared to the neutral position of the vehicle in multicopter mode
* we need to swap the roll and the yaw axis (1st and 3rd column) in the rotation matrix.
* Additionally, in order to get the correct sign of the pitch, we need to multiply
* the new x axis of the rotation matrix with -1
*
* original: modified:
*
* Rxx Ryx Rzx -Rzx Ryx Rxx
* Rxy Ryy Rzy -Rzy Ryy Rxy
* Rxz Ryz Rzz -Rzz Ryz Rxz
* */
matrix::Dcmf R_adapted = R; //modified rotation matrix
/* move z to x */
R_adapted(0, 0) = R(0, 2);
R_adapted(1, 0) = R(1, 2);
R_adapted(2, 0) = R(2, 2);
/* move x to z */
R_adapted(0, 2) = R(0, 0);
R_adapted(1, 2) = R(1, 0);
R_adapted(2, 2) = R(2, 0);
/* change direction of pitch (convert to right handed system) */
R_adapted(0, 0) = -R_adapted(0, 0);
R_adapted(1, 0) = -R_adapted(1, 0);
R_adapted(2, 0) = -R_adapted(2, 0);
/* fill in new attitude data */
R = R_adapted;
/* lastly, roll- and yawspeed have to be swaped */
float helper = rollspeed;
rollspeed = -yawspeed;
yawspeed = helper;
if (_att_sp.pitch_reset_integral) {
_pitch_ctrl.reset_integrator();
}
const matrix::Eulerf euler_angles(R);
if (_att_sp.yaw_reset_integral) {
_yaw_ctrl.reset_integrator();
_wheel_ctrl.reset_integrator();
}
vehicle_attitude_setpoint_poll();
vehicle_control_mode_poll();
vehicle_manual_poll();
_global_pos_sub.update(&_global_pos);
vehicle_status_poll();
vehicle_land_detected_poll();
/* Reset integrators if the aircraft is on ground
* or a multicopter (but not transitioning VTOL)
*/
if (_landed
|| (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
&& !_vehicle_status.in_transition_mode)) {
// 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;
_roll_ctrl.reset_integrator();
_pitch_ctrl.reset_integrator();
_yaw_ctrl.reset_integrator();
_wheel_ctrl.reset_integrator();
}
/* 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);
/* Prepare data for attitude controllers */
struct ECL_ControlData control_input = {};
control_input.roll = euler_angles.phi();
control_input.pitch = euler_angles.theta();
control_input.yaw = euler_angles.psi();
control_input.body_x_rate = rollspeed;
control_input.body_y_rate = pitchspeed;
control_input.body_z_rate = yawspeed;
control_input.roll_setpoint = _att_sp.roll_body;
control_input.pitch_setpoint = _att_sp.pitch_body;
control_input.yaw_setpoint = _att_sp.yaw_body;
control_input.airspeed_min = _parameters.airspeed_min;
control_input.airspeed_max = _parameters.airspeed_max;
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;
/* Simple handling of failsafe: deploy parachute if failsafe is on */
if (_vcontrol_mode.flag_control_termination_enabled) {
_actuators_airframe.control[7] = 1.0f;
/* reset body angular rate limits on mode change */
if ((_vcontrol_mode.flag_control_attitude_enabled != _flag_control_attitude_enabled_last) || params_updated) {
if (_vcontrol_mode.flag_control_attitude_enabled
|| _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
_roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax));
_pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos));
_pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg));
_yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax));
} else {
_roll_ctrl.set_max_rate(_parameters.acro_max_x_rate_rad);
_pitch_ctrl.set_max_rate_pos(_parameters.acro_max_y_rate_rad);
_pitch_ctrl.set_max_rate_neg(_parameters.acro_max_y_rate_rad);
_yaw_ctrl.set_max_rate(_parameters.acro_max_z_rate_rad);
}
}
_flag_control_attitude_enabled_last = _vcontrol_mode.flag_control_attitude_enabled;
/* bi-linear interpolation over airspeed for actuator trim scheduling */
float trim_roll = _parameters.trim_roll;
float trim_pitch = _parameters.trim_pitch;
float trim_yaw = _parameters.trim_yaw;
if (airspeed < _parameters.airspeed_trim) {
trim_roll += math::gradual(airspeed, _parameters.airspeed_min, _parameters.airspeed_trim, _parameters.dtrim_roll_vmin,
0.0f);
trim_pitch += math::gradual(airspeed, _parameters.airspeed_min, _parameters.airspeed_trim, _parameters.dtrim_pitch_vmin,
0.0f);
trim_yaw += math::gradual(airspeed, _parameters.airspeed_min, _parameters.airspeed_trim, _parameters.dtrim_yaw_vmin,
0.0f);
} else {
_actuators_airframe.control[7] = 0.0f;
trim_roll += math::gradual(airspeed, _parameters.airspeed_trim, _parameters.airspeed_max, 0.0f,
_parameters.dtrim_roll_vmax);
trim_pitch += math::gradual(airspeed, _parameters.airspeed_trim, _parameters.airspeed_max, 0.0f,
_parameters.dtrim_pitch_vmax);
trim_yaw += math::gradual(airspeed, _parameters.airspeed_trim, _parameters.airspeed_max, 0.0f,
_parameters.dtrim_yaw_vmax);
}
/* 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) {
continue;
}
/* add trim increment if flaps are deployed */
trim_roll += _flaps_applied * _parameters.dtrim_roll_flaps;
trim_pitch += _flaps_applied * _parameters.dtrim_pitch_flaps;
control_flaps(deltaT);
/* 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);
_yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude
_wheel_ctrl.control_attitude(control_input);
/* decide if in stabilized or full manual control */
if (_vcontrol_mode.flag_control_rates_enabled) {
/* Update input data for rate controllers */
control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate();
control_input.pitch_rate_setpoint = _pitch_ctrl.get_desired_rate();
control_input.yaw_rate_setpoint = _yaw_ctrl.get_desired_rate();
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 = (_parameters.airspeed_min * 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();
}
if (_att_sp.pitch_reset_integral) {
_pitch_ctrl.reset_integrator();
}
if (_att_sp.yaw_reset_integral) {
_yaw_ctrl.reset_integrator();
_wheel_ctrl.reset_integrator();
}
/* Reset integrators if the aircraft is on ground
* or a multicopter (but not transitioning VTOL)
*/
if (_landed
|| (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
&& !_vehicle_status.in_transition_mode)) {
_roll_ctrl.reset_integrator();
_pitch_ctrl.reset_integrator();
_yaw_ctrl.reset_integrator();
_wheel_ctrl.reset_integrator();
}
/* Prepare data for attitude controllers */
struct ECL_ControlData control_input = {};
control_input.roll = euler_angles.phi();
control_input.pitch = euler_angles.theta();
control_input.yaw = euler_angles.psi();
control_input.body_x_rate = rollspeed;
control_input.body_y_rate = pitchspeed;
control_input.body_z_rate = yawspeed;
control_input.roll_setpoint = _att_sp.roll_body;
control_input.pitch_setpoint = _att_sp.pitch_body;
control_input.yaw_setpoint = _att_sp.yaw_body;
control_input.airspeed_min = _parameters.airspeed_min;
control_input.airspeed_max = _parameters.airspeed_max;
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;
/* reset body angular rate limits on mode change */
if ((_vcontrol_mode.flag_control_attitude_enabled != _flag_control_attitude_enabled_last) || params_updated) {
if (_vcontrol_mode.flag_control_attitude_enabled
|| _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
_roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax));
_pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos));
_pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg));
_yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax));
} else {
_roll_ctrl.set_max_rate(_parameters.acro_max_x_rate_rad);
_pitch_ctrl.set_max_rate_pos(_parameters.acro_max_y_rate_rad);
_pitch_ctrl.set_max_rate_neg(_parameters.acro_max_y_rate_rad);
_yaw_ctrl.set_max_rate(_parameters.acro_max_z_rate_rad);
}
}
_flag_control_attitude_enabled_last = _vcontrol_mode.flag_control_attitude_enabled;
/* bi-linear interpolation over airspeed for actuator trim scheduling */
float trim_roll = _parameters.trim_roll;
float trim_pitch = _parameters.trim_pitch;
float trim_yaw = _parameters.trim_yaw;
if (airspeed < _parameters.airspeed_trim) {
trim_roll += math::gradual(airspeed, _parameters.airspeed_min, _parameters.airspeed_trim, _parameters.dtrim_roll_vmin,
0.0f);
trim_pitch += math::gradual(airspeed, _parameters.airspeed_min, _parameters.airspeed_trim, _parameters.dtrim_pitch_vmin,
0.0f);
trim_yaw += math::gradual(airspeed, _parameters.airspeed_min, _parameters.airspeed_trim, _parameters.dtrim_yaw_vmin,
0.0f);
} else {
trim_roll += math::gradual(airspeed, _parameters.airspeed_trim, _parameters.airspeed_max, 0.0f,
_parameters.dtrim_roll_vmax);
trim_pitch += math::gradual(airspeed, _parameters.airspeed_trim, _parameters.airspeed_max, 0.0f,
_parameters.dtrim_pitch_vmax);
trim_yaw += math::gradual(airspeed, _parameters.airspeed_trim, _parameters.airspeed_max, 0.0f,
_parameters.dtrim_yaw_vmax);
}
/* add trim increment if flaps are deployed */
trim_roll += _flaps_applied * _parameters.dtrim_roll_flaps;
trim_pitch += _flaps_applied * _parameters.dtrim_pitch_flaps;
/* 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);
_yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude
_wheel_ctrl.control_attitude(control_input);
/* Update input data for rate controllers */
control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate();
control_input.pitch_rate_setpoint = _pitch_ctrl.get_desired_rate();
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);
_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();
perf_count(_nonfinite_output_perf);
}
float pitch_u = _pitch_ctrl.control_euler_rate(control_input);
_actuators.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch : trim_pitch;
if (!PX4_ISFINITE(pitch_u)) {
_pitch_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
}
float yaw_u = 0.0f;
if (_parameters.w_en && _att_sp.fw_control_yaw) {
yaw_u = _wheel_ctrl.control_bodyrate(control_input);
} else {
yaw_u = _yaw_ctrl.control_euler_rate(control_input);
}
_actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw;
/* add in manual rudder control in manual modes */
if (_vcontrol_mode.flag_control_manual_enabled) {
_actuators.control[actuator_controls_s::INDEX_YAW] += _manual.r;
}
if (!PX4_ISFINITE(yaw_u)) {
_yaw_ctrl.reset_integrator();
_wheel_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
}
/* throttle passed through if it is finite and if no engine failure was detected */
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = (PX4_ISFINITE(_att_sp.thrust_body[0])
&& !_vehicle_status.engine_failure) ? _att_sp.thrust_body[0] : 0.0f;
/* scale effort by battery status */
if (_parameters.bat_scale_en &&
_actuators.control[actuator_controls_s::INDEX_THROTTLE] > 0.1f) {
if (_battery_status_sub.updated()) {
battery_status_s battery_status{};
if (_battery_status_sub.copy(&battery_status)) {
if (battery_status.scale > 0.0f) {
_battery_scale = battery_status.scale;
}
}
}
_actuators.control[actuator_controls_s::INDEX_THROTTLE] *= _battery_scale;
}
} else {
perf_count(_nonfinite_input_perf);
}
/*
* Lazily publish the rate setpoint (for analysis, the actuators are published below)
* only once available
*/
_rates_sp.roll = _roll_ctrl.get_desired_bodyrate();
_rates_sp.pitch = _pitch_ctrl.get_desired_bodyrate();
_rates_sp.yaw = _yaw_ctrl.get_desired_bodyrate();
_rates_sp.timestamp = hrt_absolute_time();
if (_rate_sp_pub != nullptr) {
/* publish the attitude rates setpoint */
orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &_rates_sp);
} else {
/* advertise the attitude rates setpoint */
_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp);
}
} else {
vehicle_rates_setpoint_poll();
_roll_ctrl.set_bodyrate_setpoint(_rates_sp.roll);
_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);
/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
float roll_u = _roll_ctrl.control_euler_rate(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);
if (!PX4_ISFINITE(roll_u)) {
_roll_ctrl.reset_integrator();
}
float pitch_u = _pitch_ctrl.control_euler_rate(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);
if (!PX4_ISFINITE(pitch_u)) {
_pitch_ctrl.reset_integrator();
}
float yaw_u = 0.0f;
if (_parameters.w_en && _att_sp.fw_control_yaw) {
yaw_u = _wheel_ctrl.control_bodyrate(control_input);
} else {
yaw_u = _yaw_ctrl.control_euler_rate(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]) ?
_rates_sp.thrust_body[0] : 0.0f;
/* add in manual rudder control in manual modes */
if (_vcontrol_mode.flag_control_manual_enabled) {
_actuators.control[actuator_controls_s::INDEX_YAW] += _manual.r;
}
if (!PX4_ISFINITE(yaw_u)) {
_yaw_ctrl.reset_integrator();
_wheel_ctrl.reset_integrator();
}
/* throttle passed through if it is finite and if no engine failure was detected */
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = (PX4_ISFINITE(_att_sp.thrust_body[0])
&& !_vehicle_status.engine_failure) ? _att_sp.thrust_body[0] : 0.0f;
/* scale effort by battery status */
if (_parameters.bat_scale_en &&
_actuators.control[actuator_controls_s::INDEX_THROTTLE] > 0.1f) {
if (_battery_status_sub.updated()) {
battery_status_s battery_status{};
if (_battery_status_sub.copy(&battery_status)) {
if (battery_status.scale > 0.0f) {
_battery_scale = battery_status.scale;
}
}
}
_actuators.control[actuator_controls_s::INDEX_THROTTLE] *= _battery_scale;
}
}
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();
/*
* Lazily publish the rate setpoint (for analysis, the actuators are published below)
* only once available
*/
_rates_sp.roll = _roll_ctrl.get_desired_bodyrate();
_rates_sp.pitch = _pitch_ctrl.get_desired_bodyrate();
_rates_sp.yaw = _yaw_ctrl.get_desired_bodyrate();
int instance;
orb_publish_auto(ORB_ID(rate_ctrl_status), &_rate_ctrl_status_pub, &rate_ctrl_status, &instance, ORB_PRIO_DEFAULT);
}
_rates_sp.timestamp = hrt_absolute_time();
// Add feed-forward from roll control output to yaw control output
// This can be used to counteract the adverse yaw effect when rolling the plane
_actuators.control[actuator_controls_s::INDEX_YAW] += _parameters.roll_to_yaw_ff * math::constrain(
_actuators.control[actuator_controls_s::INDEX_ROLL], -1.0f, 1.0f);
_actuators.control[actuator_controls_s::INDEX_FLAPS] = _flaps_applied;
_actuators.control[5] = _manual.aux1;
_actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = _flaperons_applied;
// FIXME: this should use _vcontrol_mode.landing_gear_pos in the future
_actuators.control[7] = _manual.aux3;
/* lazily publish the setpoint only once available */
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _att.timestamp;
_actuators_airframe.timestamp = hrt_absolute_time();
_actuators_airframe.timestamp_sample = _att.timestamp;
/* Only publish if any of the proper modes are enabled */
if (_vcontrol_mode.flag_control_rates_enabled ||
_vcontrol_mode.flag_control_attitude_enabled ||
_vcontrol_mode.flag_control_manual_enabled) {
/* publish the actuator controls */
if (_actuators_0_pub != nullptr) {
orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
} else if (_actuators_id) {
_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
}
if (_actuators_2_pub != nullptr) {
/* publish the actuator controls*/
orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe);
if (_rate_sp_pub != nullptr) {
/* publish the attitude rates setpoint */
orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &_rates_sp);
} else {
/* advertise and publish */
_actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe);
/* advertise the attitude rates setpoint */
_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp);
}
} else {
vehicle_rates_setpoint_poll();
_roll_ctrl.set_bodyrate_setpoint(_rates_sp.roll);
_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);
_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);
_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);
_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]) ?
_rates_sp.thrust_body[0] : 0.0f;
}
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();
int instance;
orb_publish_auto(ORB_ID(rate_ctrl_status), &_rate_ctrl_status_pub, &rate_ctrl_status, &instance, ORB_PRIO_DEFAULT);
}
perf_end(_loop_perf);
// Add feed-forward from roll control output to yaw control output
// This can be used to counteract the adverse yaw effect when rolling the plane
_actuators.control[actuator_controls_s::INDEX_YAW] += _parameters.roll_to_yaw_ff * math::constrain(
_actuators.control[actuator_controls_s::INDEX_ROLL], -1.0f, 1.0f);
_actuators.control[actuator_controls_s::INDEX_FLAPS] = _flaps_applied;
_actuators.control[5] = _manual.aux1;
_actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = _flaperons_applied;
// FIXME: this should use _vcontrol_mode.landing_gear_pos in the future
_actuators.control[7] = _manual.aux3;
/* lazily publish the setpoint only once available */
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _att.timestamp;
_actuators_airframe.timestamp = hrt_absolute_time();
_actuators_airframe.timestamp_sample = _att.timestamp;
/* Only publish if any of the proper modes are enabled */
if (_vcontrol_mode.flag_control_rates_enabled ||
_vcontrol_mode.flag_control_attitude_enabled ||
_vcontrol_mode.flag_control_manual_enabled) {
/* publish the actuator controls */
if (_actuators_0_pub != nullptr) {
orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
} else if (_actuators_id) {
_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
}
if (_actuators_2_pub != nullptr) {
/* publish the actuator controls*/
orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe);
} else {
/* advertise and publish */
_actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe);
}
}
}
perf_end(_loop_perf);
}
void FixedwingAttitudeControl::control_flaps(const float dt)
@@ -917,26 +888,27 @@ void FixedwingAttitudeControl::control_flaps(const float dt)
}
}
FixedwingAttitudeControl *FixedwingAttitudeControl::instantiate(int argc, char *argv[])
{
return new FixedwingAttitudeControl();
}
int FixedwingAttitudeControl::task_spawn(int argc, char *argv[])
{
_task_id = px4_task_spawn_cmd("fw_att_controol",
SCHED_DEFAULT,
SCHED_PRIORITY_ATTITUDE_CONTROL,
1500,
(px4_main_t)&run_trampoline,
(char *const *)argv);
FixedwingAttitudeControl *instance = new FixedwingAttitudeControl();
if (_task_id < 0) {
_task_id = -1;
return -errno;
if (instance) {
_object.store(instance);
_task_id = task_id_is_work_queue;
if (instance->init()) {
return PX4_OK;
}
} else {
PX4_ERR("alloc failed");
}
return 0;
delete instance;
_object.store(nullptr);
_task_id = -1;
return PX4_ERROR;
}
int FixedwingAttitudeControl::custom_command(int argc, char *argv[])
@@ -970,7 +942,8 @@ int FixedwingAttitudeControl::print_status()
{
PX4_INFO("Running");
// perf?
perf_print_counter(_loop_perf);
perf_print_counter(_loop_interval_perf);
return 0;
}
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
+10 -11
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -46,7 +46,9 @@
#include <px4_tasks.h>
#include <parameters/param.h>
#include <perf/perf_counter.h>
#include <px4_work_queue/WorkItem.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
@@ -67,7 +69,7 @@ using matrix::Quatf;
using uORB::SubscriptionData;
class FixedwingAttitudeControl final : public ModuleBase<FixedwingAttitudeControl>
class FixedwingAttitudeControl final : public ModuleBase<FixedwingAttitudeControl>, public px4::WorkItem
{
public:
FixedwingAttitudeControl();
@@ -76,24 +78,22 @@ public:
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static FixedwingAttitudeControl *instantiate(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
/** @see ModuleBase::run() */
void run() override;
/** @see ModuleBase::print_status() */
int print_status() override;
void Run() override;
bool init();
private:
int _att_sub{-1}; /**< vehicle attitude */
uORB::SubscriptionCallbackWorkItem _att_sub{this, ORB_ID(vehicle_attitude)}; /**< vehicle attitude */
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 */
@@ -128,8 +128,7 @@ private:
vehicle_status_s _vehicle_status {}; /**< vehicle status */
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */
perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */
perf_counter_t _loop_interval_perf; /**< loop interval performance counter */
float _flaps_applied{0.0f};
float _flaperons_applied{0.0f};