diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index c75588aa1a..e102c71eb5 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -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; } diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index acb7b4280b..adc2febd44 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -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 #include #include +#include #include +#include #include #include #include @@ -67,7 +69,7 @@ using matrix::Quatf; using uORB::SubscriptionData; -class FixedwingAttitudeControl final : public ModuleBase +class FixedwingAttitudeControl final : public ModuleBase, 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};