/**************************************************************************** * * 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 * are met: * * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ #include "AngularVelocityController.hpp" #include #include #include #include #include using namespace matrix; using namespace time_literals; AngularVelocityController::AngularVelocityController() : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::ctrl_alloc), _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) { _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; parameters_updated(); } AngularVelocityController::~AngularVelocityController() { perf_free(_loop_perf); } bool AngularVelocityController::init() { if (!_vehicle_angular_velocity_sub.registerCallback()) { PX4_ERR("vehicle_angular_velocity callback registration failed!"); return false; } return true; } void AngularVelocityController::parameters_updated() { // Control parameters // The controller gain K is used to convert the parallel (P + I/s + sD) form // to the ideal (K * [1 + 1/sTi + sTd]) form const Vector3f k_gains = Vector3f(_param_avc_x_k.get(), _param_avc_y_k.get(), _param_avc_z_k.get()); _control.setGains( k_gains.emult(Vector3f(_param_avc_x_p.get(), _param_avc_y_p.get(), _param_avc_z_p.get())), k_gains.emult(Vector3f(_param_avc_x_i.get(), _param_avc_y_i.get(), _param_avc_z_i.get())), k_gains.emult(Vector3f(_param_avc_x_d.get(), _param_avc_y_d.get(), _param_avc_z_d.get()))); _control.setIntegratorLimit( Vector3f(_param_avc_x_i_lim.get(), _param_avc_y_i_lim.get(), _param_avc_z_i_lim.get())); _control.setFeedForwardGain( Vector3f(_param_avc_x_ff.get(), _param_avc_y_ff.get(), _param_avc_z_ff.get())); // inertia matrix const float inertia[3][3] = { {_param_vm_inertia_xx.get(), _param_vm_inertia_xy.get(), _param_vm_inertia_xz.get()}, {_param_vm_inertia_xy.get(), _param_vm_inertia_yy.get(), _param_vm_inertia_yz.get()}, {_param_vm_inertia_xz.get(), _param_vm_inertia_yz.get(), _param_vm_inertia_zz.get()} }; _control.setInertiaMatrix(matrix::Matrix3f(inertia)); // Hover thrust if (!_param_mpc_use_hte.get()) { _hover_thrust = _param_mpc_thr_hover.get(); } } void AngularVelocityController::Run() { if (should_exit()) { _vehicle_angular_velocity_sub.unregisterCallback(); exit_and_cleanup(); return; } perf_begin(_loop_perf); // Check if parameters have changed if (_parameter_update_sub.updated()) { // clear update parameter_update_s param_update; _parameter_update_sub.copy(¶m_update); updateParams(); parameters_updated(); } /* run controller on gyro changes */ vehicle_angular_velocity_s vehicle_angular_velocity; if (_vehicle_angular_velocity_sub.update(&vehicle_angular_velocity)) { const hrt_abstime now = hrt_absolute_time(); _timestamp_sample = vehicle_angular_velocity.timestamp_sample; // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f); _last_run = now; const Vector3f angular_velocity{vehicle_angular_velocity.xyz}; /* check for updates in other topics */ _vehicle_status_sub.update(&_vehicle_status); _vehicle_control_mode_sub.update(&_vehicle_control_mode); if (_vehicle_land_detected_sub.updated()) { vehicle_land_detected_s vehicle_land_detected; if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) { _landed = vehicle_land_detected.landed; _maybe_landed = vehicle_land_detected.maybe_landed; } } // Check for updates of hover thrust if (_param_mpc_use_hte.get()) { hover_thrust_estimate_s hte; if (_hover_thrust_estimate_sub.update(&hte)) { _hover_thrust = hte.hover_thrust; } } // check angular acceleration topic vehicle_angular_acceleration_s vehicle_angular_acceleration; if (_vehicle_angular_acceleration_sub.update(&vehicle_angular_acceleration)) { _angular_acceleration = Vector3f(vehicle_angular_acceleration.xyz); } // check rates setpoint topic vehicle_rates_setpoint_s vehicle_rates_setpoint; if (_vehicle_rates_setpoint_sub.update(&vehicle_rates_setpoint)) { _angular_velocity_sp(0) = vehicle_rates_setpoint.roll; _angular_velocity_sp(1) = vehicle_rates_setpoint.pitch; _angular_velocity_sp(2) = vehicle_rates_setpoint.yaw; _thrust_sp = Vector3f(vehicle_rates_setpoint.thrust_body); // Scale _thrust_sp in Newton, assuming _hover_thrust is equivalent to 1G _thrust_sp *= (_param_vm_mass.get() * CONSTANTS_ONE_G / _hover_thrust); } // run the controller if (_vehicle_control_mode.flag_control_rates_enabled) { // reset integral if disarmed if (!_vehicle_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { _control.resetIntegral(); } // update saturation status from mixer feedback control_allocator_status_s control_allocator_status; if (_control_allocator_status_sub.update(&control_allocator_status)) { Vector saturation_positive; Vector saturation_negative; if (!control_allocator_status.torque_setpoint_achieved) { for (size_t i = 0; i < 3; i++) { if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) { saturation_positive(i) = true; } else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) { saturation_negative(i) = true; } } } _control.setSaturationStatus(saturation_positive, saturation_negative); } // run rate controller _control.update(angular_velocity, _angular_velocity_sp, _angular_acceleration, dt, _maybe_landed || _landed); // publish rate controller status rate_ctrl_status_s rate_ctrl_status{}; Vector3f integral = _control.getIntegral(); rate_ctrl_status.timestamp = hrt_absolute_time(); rate_ctrl_status.rollspeed_integ = integral(0); rate_ctrl_status.pitchspeed_integ = integral(1); rate_ctrl_status.yawspeed_integ = integral(2); _rate_ctrl_status_pub.publish(rate_ctrl_status); // publish controller output publish_angular_acceleration_setpoint(); publish_torque_setpoint(); publish_thrust_setpoint(); } } perf_end(_loop_perf); } void AngularVelocityController::publish_angular_acceleration_setpoint() { Vector3f angular_accel_sp = _control.getAngularAccelerationSetpoint(); vehicle_angular_acceleration_setpoint_s v_angular_accel_sp = {}; v_angular_accel_sp.timestamp = hrt_absolute_time(); v_angular_accel_sp.timestamp_sample = _timestamp_sample; v_angular_accel_sp.xyz[0] = (PX4_ISFINITE(angular_accel_sp(0))) ? angular_accel_sp(0) : 0.0f; v_angular_accel_sp.xyz[1] = (PX4_ISFINITE(angular_accel_sp(1))) ? angular_accel_sp(1) : 0.0f; v_angular_accel_sp.xyz[2] = (PX4_ISFINITE(angular_accel_sp(2))) ? angular_accel_sp(2) : 0.0f; _vehicle_angular_acceleration_setpoint_pub.publish(v_angular_accel_sp); } void AngularVelocityController::publish_torque_setpoint() { Vector3f torque_sp = _control.getTorqueSetpoint(); vehicle_torque_setpoint_s v_torque_sp = {}; v_torque_sp.timestamp = hrt_absolute_time(); v_torque_sp.timestamp_sample = _timestamp_sample; v_torque_sp.xyz[0] = (PX4_ISFINITE(torque_sp(0))) ? torque_sp(0) : 0.0f; v_torque_sp.xyz[1] = (PX4_ISFINITE(torque_sp(1))) ? torque_sp(1) : 0.0f; v_torque_sp.xyz[2] = (PX4_ISFINITE(torque_sp(2))) ? torque_sp(2) : 0.0f; _vehicle_torque_setpoint_pub.publish(v_torque_sp); } void AngularVelocityController::publish_thrust_setpoint() { vehicle_thrust_setpoint_s v_thrust_sp = {}; v_thrust_sp.timestamp = hrt_absolute_time(); v_thrust_sp.timestamp_sample = _timestamp_sample; v_thrust_sp.xyz[0] = (PX4_ISFINITE(_thrust_sp(0))) ? (_thrust_sp(0)) : 0.0f; v_thrust_sp.xyz[1] = (PX4_ISFINITE(_thrust_sp(1))) ? (_thrust_sp(1)) : 0.0f; v_thrust_sp.xyz[2] = (PX4_ISFINITE(_thrust_sp(2))) ? (_thrust_sp(2)) : 0.0f; _vehicle_thrust_setpoint_pub.publish(v_thrust_sp); } int AngularVelocityController::task_spawn(int argc, char *argv[]) { AngularVelocityController *instance = new AngularVelocityController(); if (instance) { _object.store(instance); _task_id = task_id_is_work_queue; if (instance->init()) { return PX4_OK; } } else { PX4_ERR("alloc failed"); } delete instance; _object.store(nullptr); _task_id = -1; return PX4_ERROR; } int AngularVelocityController::print_status() { PX4_INFO("Running"); perf_print_counter(_loop_perf); return 0; } int AngularVelocityController::custom_command(int argc, char *argv[]) { return print_usage("unknown command"); } int AngularVelocityController::print_usage(const char *reason) { if (reason) { PX4_WARN("%s\n", reason); } PRINT_MODULE_DESCRIPTION( R"DESCR_STR( ### Description This implements the angular velocity controller. It takes angular velocity setpoints and measured angular velocity as inputs and outputs actuator setpoints. The controller has a PID loop for angular rate error. )DESCR_STR"); PRINT_MODULE_USAGE_NAME(MODULE_NAME, "controller"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; } /** * Angular velocity controller app start / stop handling function */ extern "C" __EXPORT int angular_velocity_controller_main(int argc, char *argv[]); int angular_velocity_controller_main(int argc, char *argv[]) { return AngularVelocityController::main(argc, argv); }