fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-02 07:00:36 +08:00
Code Issues Packages Projects Releases Wiki Activity
Files
55da07d3c42ac48f16c7a587ab42dcc73ef507d2
PX4-Autopilot/src/modules/mc_att_control/mc_att_control_main.cpp
T
Beat Küng 55da07d3c4 mc_att_control: fix computation of yaw weight for attitude control 
Previously, the yaw weight was based on the tilt angle of the attitude
setpoint (R_sp(2, 2) == cos(tilt angle)). This makes no sense, it means
the weight is low for high tilt angles even if there is no roll-pitch error
at all.

This patch changes the weight computation to be based on the tilt angle
error: the yaw weight is 1 if there is no roll-pitch error (independent
from current tilt angle), and is reduced for higher tilt angle errors.

The weight was added in 05e9a30573, without any explanation or
derivation of how and why the weight is chosen that way.

However this patch works much better in practice. The yaw control is
improved, though it can be a bit slow to converge if you do continuous &
fast roll-pitch motions (which is expected).
2017-10-01 15:36:21 +02:00

1458 lines
46 KiB
C++
Raw Blame History

/****************************************************************************
*
* Copyright (c) 2013-2017 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.
*
****************************************************************************/
/**
* @file mc_att_control_main.cpp
* Multicopter attitude controller.
*
* Publication for the desired attitude tracking:
* Daniel Mellinger and Vijay Kumar. Minimum Snap Trajectory Generation and Control for Quadrotors.
* Int. Conf. on Robotics and Automation, Shanghai, China, May 2011.
*
* @author Lorenz Meier <lorenz@px4.io>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Sander Smeets <sander@droneslab.com>
*
* The controller has two loops: P loop for angular error and PD loop for angular rate error.
* Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch.
* For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw,
* so actual rotation axis is not constant. For large deviations controller rotates copter around fixed axis.
* These two approaches fused seamlessly with weight depending on angular error.
* When thrust vector directed near-horizontally (e.g. roll ~= PI/2) yaw setpoint ignored because of singularity.
* Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging.
* If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers.
*/
#include <conversion/rotation.h>
#include <drivers/drv_hrt.h>
#include <lib/geo/geo.h>
#include <lib/mathlib/mathlib.h>
#include <lib/tailsitter_recovery/tailsitter_recovery.h>
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_posix.h>
#include <px4_tasks.h>
#include <systemlib/circuit_breaker.h>
#include <systemlib/err.h>
#include <systemlib/mixer/mixer.h>
#include <systemlib/param/param.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/mc_att_ctrl_status.h>
#include <uORB/topics/multirotor_motor_limits.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_bias.h>
#include <uORB/topics/sensor_correction.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/uORB.h>
/**
* Multicopter attitude control app start / stop handling function
*
* @ingroup apps
*/
extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]);
#define MIN_TAKEOFF_THRUST 0.2f
#define TPA_RATE_LOWER_LIMIT 0.05f
#define ATTITUDE_TC_DEFAULT 0.2f
#define AXIS_INDEX_ROLL 0
#define AXIS_INDEX_PITCH 1
#define AXIS_INDEX_YAW 2
#define AXIS_COUNT 3
#define MAX_GYRO_COUNT 3
class MulticopterAttitudeControl
{
public:
/**
* Constructor
*/
MulticopterAttitudeControl();
/**
* Destructor, also kills the main task
*/
~MulticopterAttitudeControl();
/**
* Start the multicopter attitude control task.
*
* @return OK on success.
*/
int start();
private:
bool _task_should_exit; /**< if true, task_main() should exit */
int _control_task; /**< task handle */
int _v_att_sub; /**< vehicle attitude subscription */
int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */
int _v_rates_sp_sub; /**< vehicle rates setpoint subscription */
int _v_control_mode_sub; /**< vehicle control mode subscription */
int _params_sub; /**< parameter updates subscription */
int _manual_control_sp_sub; /**< manual control setpoint subscription */
int _armed_sub; /**< arming status subscription */
int _vehicle_status_sub; /**< vehicle status subscription */
int _motor_limits_sub; /**< motor limits subscription */
int _battery_status_sub; /**< battery status subscription */
int _sensor_gyro_sub[MAX_GYRO_COUNT]; /**< gyro data subscription */
int _sensor_correction_sub; /**< sensor thermal correction subscription */
int _sensor_bias_sub; /**< sensor in-run bias correction subscription */
unsigned _gyro_count;
int _selected_gyro;
orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */
orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */
orb_advert_t _controller_status_pub; /**< controller status publication */
orb_id_t _rates_sp_id; /**< pointer to correct rates setpoint uORB metadata structure */
orb_id_t _actuators_id; /**< pointer to correct actuator controls0 uORB metadata structure */
bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */
struct vehicle_attitude_s _v_att; /**< vehicle attitude */
struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */
struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */
struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */
struct vehicle_control_mode_s _v_control_mode; /**< vehicle control mode */
struct actuator_controls_s _actuators; /**< actuator controls */
struct actuator_armed_s _armed; /**< actuator arming status */
struct vehicle_status_s _vehicle_status; /**< vehicle status */
struct mc_att_ctrl_status_s _controller_status; /**< controller status */
struct battery_status_s _battery_status; /**< battery status */
struct sensor_gyro_s _sensor_gyro; /**< gyro data before thermal correctons and ekf bias estimates are applied */
struct sensor_correction_s _sensor_correction; /**< sensor thermal corrections */
struct sensor_bias_s _sensor_bias; /**< sensor in-run bias corrections */
MultirotorMixer::saturation_status _saturation_status{};
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _controller_latency_perf;
math::Vector<3> _rates_prev; /**< angular rates on previous step */
math::Vector<3> _rates_sp_prev; /**< previous rates setpoint */
math::Vector<3> _rates_sp; /**< angular rates setpoint */
math::Vector<3> _rates_int; /**< angular rates integral error */
float _thrust_sp; /**< thrust setpoint */
math::Vector<3> _att_control; /**< attitude control vector */
math::Matrix<3, 3> _I; /**< identity matrix */
math::Matrix<3, 3> _board_rotation = {}; /**< rotation matrix for the orientation that the board is mounted */
struct {
param_t roll_p;
param_t roll_rate_p;
param_t roll_rate_i;
param_t roll_rate_integ_lim;
param_t roll_rate_d;
param_t roll_rate_ff;
param_t pitch_p;
param_t pitch_rate_p;
param_t pitch_rate_i;
param_t pitch_rate_integ_lim;
param_t pitch_rate_d;
param_t pitch_rate_ff;
param_t tpa_breakpoint_p;
param_t tpa_breakpoint_i;
param_t tpa_breakpoint_d;
param_t tpa_rate_p;
param_t tpa_rate_i;
param_t tpa_rate_d;
param_t yaw_p;
param_t yaw_rate_p;
param_t yaw_rate_i;
param_t yaw_rate_integ_lim;
param_t yaw_rate_d;
param_t yaw_rate_ff;
param_t yaw_ff;
param_t roll_rate_max;
param_t pitch_rate_max;
param_t yaw_rate_max;
param_t yaw_auto_max;
param_t acro_roll_max;
param_t acro_pitch_max;
param_t acro_yaw_max;
param_t rattitude_thres;
param_t vtol_type;
param_t roll_tc;
param_t pitch_tc;
param_t vtol_opt_recovery_enabled;
param_t vtol_wv_yaw_rate_scale;
param_t bat_scale_en;
param_t board_rotation;
param_t board_offset[3];
} _params_handles; /**< handles for interesting parameters */
struct {
math::Vector<3> att_p; /**< P gain for angular error */
math::Vector<3> rate_p; /**< P gain for angular rate error */
math::Vector<3> rate_i; /**< I gain for angular rate error */
math::Vector<3> rate_int_lim; /**< integrator state limit for rate loop */
math::Vector<3> rate_d; /**< D gain for angular rate error */
math::Vector<3> rate_ff; /**< Feedforward gain for desired rates */
float yaw_ff; /**< yaw control feed-forward */
float tpa_breakpoint_p; /**< Throttle PID Attenuation breakpoint */
float tpa_breakpoint_i; /**< Throttle PID Attenuation breakpoint */
float tpa_breakpoint_d; /**< Throttle PID Attenuation breakpoint */
float tpa_rate_p; /**< Throttle PID Attenuation slope */
float tpa_rate_i; /**< Throttle PID Attenuation slope */
float tpa_rate_d; /**< Throttle PID Attenuation slope */
float roll_rate_max;
float pitch_rate_max;
float yaw_rate_max;
float yaw_auto_max;
math::Vector<3> mc_rate_max; /**< attitude rate limits in stabilized modes */
math::Vector<3> auto_rate_max; /**< attitude rate limits in auto modes */
math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */
float rattitude_thres;
int vtol_type; /**< 0 = Tailsitter, 1 = Tiltrotor, 2 = Standard airframe */
bool vtol_opt_recovery_enabled;
float vtol_wv_yaw_rate_scale; /**< Scale value [0, 1] for yaw rate setpoint */
int bat_scale_en;
int board_rotation;
float board_offset[3];
} _params;
TailsitterRecovery *_ts_opt_recovery; /**< Computes optimal rates for tailsitter recovery */
/**
* Update our local parameter cache.
*/
int parameters_update();
/**
* Check for parameter update and handle it.
*/
void parameter_update_poll();
/**
* Check for changes in vehicle control mode.
*/
void vehicle_control_mode_poll();
/**
* Check for changes in manual inputs.
*/
void vehicle_manual_poll();
/**
* Check for attitude setpoint updates.
*/
void vehicle_attitude_setpoint_poll();
/**
* Check for rates setpoint updates.
*/
void vehicle_rates_setpoint_poll();
/**
* Check for arming status updates.
*/
void arming_status_poll();
/**
* Attitude controller.
*/
void control_attitude(float dt);
/**
* Attitude rates controller.
*/
void control_attitude_rates(float dt);
/**
* Throttle PID attenuation.
*/
math::Vector<3> pid_attenuations(float tpa_breakpoint, float tpa_rate);
/**
* Check for vehicle status updates.
*/
void vehicle_status_poll();
/**
* Check for vehicle motor limits status.
*/
void vehicle_motor_limits_poll();
/**
* Check for battery status updates.
*/
void battery_status_poll();
/**
* Check for control state updates.
*/
void vehicle_attitude_poll();
/**
* Check for sensor thermal correction updates.
*/
void sensor_correction_poll();
/**
* Check for sensor in-run bias correction updates.
*/
void sensor_bias_poll();
/**
* Shim for calling task_main from task_create.
*/
static void task_main_trampoline(int argc, char *argv[]);
/**
* Main attitude control task.
*/
void task_main();
};
namespace mc_att_control
{
MulticopterAttitudeControl *g_control;
}
MulticopterAttitudeControl::MulticopterAttitudeControl() :
_task_should_exit(false),
_control_task(-1),
/* subscriptions */
_v_att_sub(-1),
_v_att_sp_sub(-1),
_v_control_mode_sub(-1),
_params_sub(-1),
_manual_control_sp_sub(-1),
_armed_sub(-1),
_vehicle_status_sub(-1),
_motor_limits_sub(-1),
_battery_status_sub(-1),
_sensor_correction_sub(-1),
_sensor_bias_sub(-1),
/* gyro selection */
_gyro_count(1),
_selected_gyro(0),
/* publications */
_v_rates_sp_pub(nullptr),
_actuators_0_pub(nullptr),
_controller_status_pub(nullptr),
_rates_sp_id(nullptr),
_actuators_id(nullptr),
_actuators_0_circuit_breaker_enabled(false),
_v_att{},
_v_att_sp{},
_v_rates_sp{},
_manual_control_sp{},
_v_control_mode{},
_actuators{},
_armed{},
_vehicle_status{},
_controller_status{},
_battery_status{},
_sensor_gyro{},
_sensor_correction{},
_sensor_bias{},
_saturation_status{},
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")),
_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")),
_ts_opt_recovery(nullptr)
{
for (uint8_t i = 0; i < MAX_GYRO_COUNT; i++) {
_sensor_gyro_sub[i] = -1;
}
_vehicle_status.is_rotary_wing = true;
_params.att_p.zero();
_params.rate_p.zero();
_params.rate_i.zero();
_params.rate_int_lim.zero();
_params.rate_d.zero();
_params.rate_ff.zero();
_params.yaw_ff = 0.0f;
_params.roll_rate_max = 0.0f;
_params.pitch_rate_max = 0.0f;
_params.yaw_rate_max = 0.0f;
_params.mc_rate_max.zero();
_params.auto_rate_max.zero();
_params.acro_rate_max.zero();
_params.rattitude_thres = 1.0f;
_params.vtol_opt_recovery_enabled = false;
_params.vtol_wv_yaw_rate_scale = 1.0f;
_params.bat_scale_en = 0;
_params.board_rotation = 0;
_params.board_offset[0] = 0.0f;
_params.board_offset[1] = 0.0f;
_params.board_offset[2] = 0.0f;
_rates_prev.zero();
_rates_sp.zero();
_rates_sp_prev.zero();
_rates_int.zero();
_thrust_sp = 0.0f;
_att_control.zero();
_I.identity();
_board_rotation.identity();
_params_handles.roll_p = param_find("MC_ROLL_P");
_params_handles.roll_rate_p = param_find("MC_ROLLRATE_P");
_params_handles.roll_rate_i = param_find("MC_ROLLRATE_I");
_params_handles.roll_rate_integ_lim = param_find("MC_RR_INT_LIM");
_params_handles.roll_rate_d = param_find("MC_ROLLRATE_D");
_params_handles.roll_rate_ff = param_find("MC_ROLLRATE_FF");
_params_handles.pitch_p = param_find("MC_PITCH_P");
_params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P");
_params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I");
_params_handles.pitch_rate_integ_lim = param_find("MC_PR_INT_LIM");
_params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D");
_params_handles.pitch_rate_ff = param_find("MC_PITCHRATE_FF");
_params_handles.tpa_breakpoint_p = param_find("MC_TPA_BREAK_P");
_params_handles.tpa_breakpoint_i = param_find("MC_TPA_BREAK_I");
_params_handles.tpa_breakpoint_d = param_find("MC_TPA_BREAK_D");
_params_handles.tpa_rate_p = param_find("MC_TPA_RATE_P");
_params_handles.tpa_rate_i = param_find("MC_TPA_RATE_I");
_params_handles.tpa_rate_d = param_find("MC_TPA_RATE_D");
_params_handles.yaw_p = param_find("MC_YAW_P");
_params_handles.yaw_rate_p = param_find("MC_YAWRATE_P");
_params_handles.yaw_rate_i = param_find("MC_YAWRATE_I");
_params_handles.yaw_rate_integ_lim = param_find("MC_YR_INT_LIM");
_params_handles.yaw_rate_d = param_find("MC_YAWRATE_D");
_params_handles.yaw_rate_ff = param_find("MC_YAWRATE_FF");
_params_handles.yaw_ff = param_find("MC_YAW_FF");
_params_handles.roll_rate_max = param_find("MC_ROLLRATE_MAX");
_params_handles.pitch_rate_max = param_find("MC_PITCHRATE_MAX");
_params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX");
_params_handles.yaw_auto_max = param_find("MC_YAWRAUTO_MAX");
_params_handles.acro_roll_max = param_find("MC_ACRO_R_MAX");
_params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX");
_params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX");
_params_handles.rattitude_thres = param_find("MC_RATT_TH");
_params_handles.vtol_type = param_find("VT_TYPE");
_params_handles.roll_tc = param_find("MC_ROLL_TC");
_params_handles.pitch_tc = param_find("MC_PITCH_TC");
_params_handles.vtol_opt_recovery_enabled = param_find("VT_OPT_RECOV_EN");
_params_handles.vtol_wv_yaw_rate_scale = param_find("VT_WV_YAWR_SCL");
_params_handles.bat_scale_en = param_find("MC_BAT_SCALE_EN");
/* rotations */
_params_handles.board_rotation = param_find("SENS_BOARD_ROT");
/* rotation offsets */
_params_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF");
_params_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF");
_params_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF");
/* fetch initial parameter values */
parameters_update();
if (_params.vtol_type == 0 && _params.vtol_opt_recovery_enabled) {
// the vehicle is a tailsitter, use optimal recovery control strategy
_ts_opt_recovery = new TailsitterRecovery();
}
/* initialize thermal corrections as we might not immediately get a topic update (only non-zero values) */
for (unsigned i = 0; i < 3; i++) {
// used scale factors to unity
_sensor_correction.gyro_scale_0[i] = 1.0f;
_sensor_correction.gyro_scale_1[i] = 1.0f;
_sensor_correction.gyro_scale_2[i] = 1.0f;
}
}
MulticopterAttitudeControl::~MulticopterAttitudeControl()
{
if (_control_task != -1) {
/* task wakes up every 100ms or so at the longest */
_task_should_exit = true;
/* wait for a second for the task to quit at our request */
unsigned i = 0;
do {
/* wait 20ms */
usleep(20000);
/* if we have given up, kill it */
if (++i > 50) {
px4_task_delete(_control_task);
break;
}
} while (_control_task != -1);
}
if (_ts_opt_recovery != nullptr) {
delete _ts_opt_recovery;
}
mc_att_control::g_control = nullptr;
}
int
MulticopterAttitudeControl::parameters_update()
{
float v;
float roll_tc, pitch_tc;
param_get(_params_handles.roll_tc, &roll_tc);
param_get(_params_handles.pitch_tc, &pitch_tc);
/* roll gains */
param_get(_params_handles.roll_p, &v);
_params.att_p(0) = v * (ATTITUDE_TC_DEFAULT / roll_tc);
param_get(_params_handles.roll_rate_p, &v);
_params.rate_p(0) = v * (ATTITUDE_TC_DEFAULT / roll_tc);
param_get(_params_handles.roll_rate_i, &v);
_params.rate_i(0) = v;
param_get(_params_handles.roll_rate_integ_lim, &v);
_params.rate_int_lim(0) = v;
param_get(_params_handles.roll_rate_d, &v);
_params.rate_d(0) = v * (ATTITUDE_TC_DEFAULT / roll_tc);
param_get(_params_handles.roll_rate_ff, &v);
_params.rate_ff(0) = v;
/* pitch gains */
param_get(_params_handles.pitch_p, &v);
_params.att_p(1) = v * (ATTITUDE_TC_DEFAULT / pitch_tc);
param_get(_params_handles.pitch_rate_p, &v);
_params.rate_p(1) = v * (ATTITUDE_TC_DEFAULT / pitch_tc);
param_get(_params_handles.pitch_rate_i, &v);
_params.rate_i(1) = v;
param_get(_params_handles.pitch_rate_integ_lim, &v);
_params.rate_int_lim(1) = v;
param_get(_params_handles.pitch_rate_d, &v);
_params.rate_d(1) = v * (ATTITUDE_TC_DEFAULT / pitch_tc);
param_get(_params_handles.pitch_rate_ff, &v);
_params.rate_ff(1) = v;
param_get(_params_handles.tpa_breakpoint_p, &_params.tpa_breakpoint_p);
param_get(_params_handles.tpa_breakpoint_i, &_params.tpa_breakpoint_i);
param_get(_params_handles.tpa_breakpoint_d, &_params.tpa_breakpoint_d);
param_get(_params_handles.tpa_rate_p, &_params.tpa_rate_p);
param_get(_params_handles.tpa_rate_i, &_params.tpa_rate_i);
param_get(_params_handles.tpa_rate_d, &_params.tpa_rate_d);
/* yaw gains */
param_get(_params_handles.yaw_p, &v);
_params.att_p(2) = v;
param_get(_params_handles.yaw_rate_p, &v);
_params.rate_p(2) = v;
param_get(_params_handles.yaw_rate_i, &v);
_params.rate_i(2) = v;
param_get(_params_handles.yaw_rate_integ_lim, &v);
_params.rate_int_lim(2) = v;
param_get(_params_handles.yaw_rate_d, &v);
_params.rate_d(2) = v;
param_get(_params_handles.yaw_rate_ff, &v);
_params.rate_ff(2) = v;
param_get(_params_handles.yaw_ff, &_params.yaw_ff);
/* angular rate limits */
param_get(_params_handles.roll_rate_max, &_params.roll_rate_max);
_params.mc_rate_max(0) = math::radians(_params.roll_rate_max);
param_get(_params_handles.pitch_rate_max, &_params.pitch_rate_max);
_params.mc_rate_max(1) = math::radians(_params.pitch_rate_max);
param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max);
_params.mc_rate_max(2) = math::radians(_params.yaw_rate_max);
/* auto angular rate limits */
param_get(_params_handles.roll_rate_max, &_params.roll_rate_max);
_params.auto_rate_max(0) = math::radians(_params.roll_rate_max);
param_get(_params_handles.pitch_rate_max, &_params.pitch_rate_max);
_params.auto_rate_max(1) = math::radians(_params.pitch_rate_max);
param_get(_params_handles.yaw_auto_max, &_params.yaw_auto_max);
_params.auto_rate_max(2) = math::radians(_params.yaw_auto_max);
/* manual rate control scale and acro mode roll/pitch rate limits */
param_get(_params_handles.acro_roll_max, &v);
_params.acro_rate_max(0) = math::radians(v);
param_get(_params_handles.acro_pitch_max, &v);
_params.acro_rate_max(1) = math::radians(v);
param_get(_params_handles.acro_yaw_max, &v);
_params.acro_rate_max(2) = math::radians(v);
/* stick deflection needed in rattitude mode to control rates not angles */
param_get(_params_handles.rattitude_thres, &_params.rattitude_thres);
param_get(_params_handles.vtol_type, &_params.vtol_type);
int tmp;
param_get(_params_handles.vtol_opt_recovery_enabled, &tmp);
_params.vtol_opt_recovery_enabled = (bool)tmp;
param_get(_params_handles.vtol_wv_yaw_rate_scale, &_params.vtol_wv_yaw_rate_scale);
param_get(_params_handles.bat_scale_en, &_params.bat_scale_en);
_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
/* rotation of the autopilot relative to the body */
param_get(_params_handles.board_rotation, &(_params.board_rotation));
/* fine adjustment of the rotation */
param_get(_params_handles.board_offset[0], &(_params.board_offset[0]));
param_get(_params_handles.board_offset[1], &(_params.board_offset[1]));
param_get(_params_handles.board_offset[2], &(_params.board_offset[2]));
/* get transformation matrix from sensor/board to body frame */
get_rot_matrix((enum Rotation)_params.board_rotation, &_board_rotation);
/* fine tune the rotation */
math::Matrix<3, 3> board_rotation_offset;
board_rotation_offset.from_euler(M_DEG_TO_RAD_F * _params.board_offset[0],
M_DEG_TO_RAD_F * _params.board_offset[1],
M_DEG_TO_RAD_F * _params.board_offset[2]);
_board_rotation = board_rotation_offset * _board_rotation;
return OK;
}
void
MulticopterAttitudeControl::parameter_update_poll()
{
bool updated;
/* Check if parameters have changed */
orb_check(_params_sub, &updated);
if (updated) {
struct parameter_update_s param_update;
orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
parameters_update();
}
}
void
MulticopterAttitudeControl::vehicle_control_mode_poll()
{
bool updated;
/* Check if vehicle control mode has changed */
orb_check(_v_control_mode_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode);
}
}
void
MulticopterAttitudeControl::vehicle_manual_poll()
{
bool updated;
/* get pilots inputs */
orb_check(_manual_control_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp);
}
}
void
MulticopterAttitudeControl::vehicle_attitude_setpoint_poll()
{
/* check if there is a new setpoint */
bool updated;
orb_check(_v_att_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp);
}
}
void
MulticopterAttitudeControl::vehicle_rates_setpoint_poll()
{
/* check if there is a new setpoint */
bool updated;
orb_check(_v_rates_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_sub, &_v_rates_sp);
}
}
void
MulticopterAttitudeControl::arming_status_poll()
{
/* check if there is a new setpoint */
bool updated;
orb_check(_armed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
}
}
void
MulticopterAttitudeControl::vehicle_status_poll()
{
/* check if there is new status information */
bool vehicle_status_updated;
orb_check(_vehicle_status_sub, &vehicle_status_updated);
if (vehicle_status_updated) {
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
/* set correct uORB ID, depending on if vehicle is VTOL or not */
if (!_rates_sp_id) {
if (_vehicle_status.is_vtol) {
_rates_sp_id = ORB_ID(mc_virtual_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_virtual_mc);
} else {
_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_0);
}
}
}
}
void
MulticopterAttitudeControl::vehicle_motor_limits_poll()
{
/* check if there is a new message */
bool updated;
orb_check(_motor_limits_sub, &updated);
if (updated) {
multirotor_motor_limits_s motor_limits = {};
orb_copy(ORB_ID(multirotor_motor_limits), _motor_limits_sub, &motor_limits);
_saturation_status.value = motor_limits.saturation_status;
}
}
void
MulticopterAttitudeControl::battery_status_poll()
{
/* check if there is a new message */
bool updated;
orb_check(_battery_status_sub, &updated);
if (updated) {
orb_copy(ORB_ID(battery_status), _battery_status_sub, &_battery_status);
}
}
void
MulticopterAttitudeControl::vehicle_attitude_poll()
{
/* check if there is a new message */
bool updated;
orb_check(_v_att_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);
}
}
void
MulticopterAttitudeControl::sensor_correction_poll()
{
/* check if there is a new message */
bool updated;
orb_check(_sensor_correction_sub, &updated);
if (updated) {
orb_copy(ORB_ID(sensor_correction), _sensor_correction_sub, &_sensor_correction);
}
/* update the latest gyro selection */
if (_sensor_correction.selected_gyro_instance < _gyro_count) {
_selected_gyro = _sensor_correction.selected_gyro_instance;
}
}
void
MulticopterAttitudeControl::sensor_bias_poll()
{
/* check if there is a new message */
bool updated;
orb_check(_sensor_bias_sub, &updated);
if (updated) {
orb_copy(ORB_ID(sensor_bias), _sensor_bias_sub, &_sensor_bias);
}
}
/**
* Attitude controller.
* Input: 'vehicle_attitude_setpoint' topics (depending on mode)
* Output: '_rates_sp' vector, '_thrust_sp'
*/
void
MulticopterAttitudeControl::control_attitude(float dt)
{
vehicle_attitude_setpoint_poll();
_thrust_sp = _v_att_sp.thrust;
/* construct attitude setpoint rotation matrix */
math::Quaternion q_sp(_v_att_sp.q_d[0], _v_att_sp.q_d[1], _v_att_sp.q_d[2], _v_att_sp.q_d[3]);
math::Matrix<3, 3> R_sp = q_sp.to_dcm();
/* get current rotation matrix from control state quaternions */
math::Quaternion q_att(_v_att.q[0], _v_att.q[1], _v_att.q[2], _v_att.q[3]);
math::Matrix<3, 3> R = q_att.to_dcm();
/* all input data is ready, run controller itself */
/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2));
math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
/* axis and sin(angle) of desired rotation (indexes: 0=pitch, 1=roll, 2=yaw).
* This is for roll/pitch only (tilt), e_R(2) is 0 */
math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);
/* calculate angle error */
float e_R_z_sin = e_R.length(); // == sin(tilt angle error)
float e_R_z_cos = R_z * R_sp_z; // == cos(tilt angle error) == (R.transposed() * R_sp)(2, 2)
/* calculate rotation matrix after roll/pitch only rotation */
math::Matrix<3, 3> R_rp;
if (e_R_z_sin > 0.0f) {
/* get axis-angle representation */
float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);
math::Vector<3> e_R_z_axis = e_R / e_R_z_sin;
e_R = e_R_z_axis * e_R_z_angle;
/* cross product matrix for e_R_axis */
math::Matrix<3, 3> e_R_cp;
e_R_cp.zero();
e_R_cp(0, 1) = -e_R_z_axis(2);
e_R_cp(0, 2) = e_R_z_axis(1);
e_R_cp(1, 0) = e_R_z_axis(2);
e_R_cp(1, 2) = -e_R_z_axis(0);
e_R_cp(2, 0) = -e_R_z_axis(1);
e_R_cp(2, 1) = e_R_z_axis(0);
/* rotation matrix for roll/pitch only rotation */
R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
} else {
/* zero roll/pitch rotation */
R_rp = R;
}
/* R_rp and R_sp have the same Z axis, calculate yaw error */
math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
/* calculate the weight for yaw control
* Make the weight depend on the tilt angle error: the higher the error of roll and/or pitch, the lower
* the weight that we use to control the yaw. This gives precedence to roll & pitch correction.
* The weight is 1 if there is no tilt error.
*/
float yaw_w = e_R_z_cos * e_R_z_cos;
/* calculate the angle between R_rp_x and R_sp_x (yaw angle error), and apply the yaw weight */
e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;
if (e_R_z_cos < 0.0f) {
/* for large thrust vector rotations use another rotation method:
* calculate angle and axis for R -> R_sp rotation directly */
math::Quaternion q_error;
q_error.from_dcm(R.transposed() * R_sp);
math::Vector<3> e_R_d = q_error(0) >= 0.0f ? q_error.imag() * 2.0f : -q_error.imag() * 2.0f;
/* use fusion of Z axis based rotation and direct rotation */
float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;
}
/* calculate angular rates setpoint */
_rates_sp = _params.att_p.emult(e_R);
/* Feed forward the yaw setpoint rate. We need to transform the yaw from world into body frame.
* The following is a simplification of:
* R.transposed() * math::Vector<3>(0.f, 0.f, _v_att_sp.yaw_sp_move_rate * _params.yaw_ff)
*/
math::Vector<3> yaw_feedforward_rate(R(2, 0), R(2, 1), R(2, 2));
yaw_feedforward_rate *= _v_att_sp.yaw_sp_move_rate * _params.yaw_ff;
yaw_feedforward_rate(2) *= yaw_w;
_rates_sp += yaw_feedforward_rate;
/* limit rates */
for (int i = 0; i < 3; i++) {
if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) &&
!_v_control_mode.flag_control_manual_enabled) {
_rates_sp(i) = math::constrain(_rates_sp(i), -_params.auto_rate_max(i), _params.auto_rate_max(i));
} else {
_rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i));
}
}
/* weather-vane mode, dampen yaw rate */
if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) &&
_v_att_sp.disable_mc_yaw_control == true && !_v_control_mode.flag_control_manual_enabled) {
float wv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale;
_rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max);
// prevent integrator winding up in weathervane mode
_rates_int(2) = 0.0f;
}
}
/*
* Throttle PID attenuation
* Function visualization available here https://www.desmos.com/calculator/gn4mfoddje
* Input: 'tpa_breakpoint', 'tpa_rate', '_thrust_sp'
* Output: 'pidAttenuationPerAxis' vector
*/
math::Vector<3>
MulticopterAttitudeControl::pid_attenuations(float tpa_breakpoint, float tpa_rate)
{
/* throttle pid attenuation factor */
float tpa = 1.0f - tpa_rate * (fabsf(_v_rates_sp.thrust) - tpa_breakpoint) / (1.0f - tpa_breakpoint);
tpa = fmaxf(TPA_RATE_LOWER_LIMIT, fminf(1.0f, tpa));
math::Vector<3> pidAttenuationPerAxis;
pidAttenuationPerAxis(AXIS_INDEX_ROLL) = tpa;
pidAttenuationPerAxis(AXIS_INDEX_PITCH) = tpa;
pidAttenuationPerAxis(AXIS_INDEX_YAW) = 1.0;
return pidAttenuationPerAxis;
}
/*
* Attitude rates controller.
* Input: '_rates_sp' vector, '_thrust_sp'
* Output: '_att_control' vector
*/
void
MulticopterAttitudeControl::control_attitude_rates(float dt)
{
/* reset integral if disarmed */
if (!_armed.armed || !_vehicle_status.is_rotary_wing) {
_rates_int.zero();
}
// get the raw gyro data and correct for thermal errors
math::Vector<3> rates;
if (_selected_gyro == 0) {
rates(0) = (_sensor_gyro.x - _sensor_correction.gyro_offset_0[0]) * _sensor_correction.gyro_scale_0[0];
rates(1) = (_sensor_gyro.y - _sensor_correction.gyro_offset_0[1]) * _sensor_correction.gyro_scale_0[1];
rates(2) = (_sensor_gyro.z - _sensor_correction.gyro_offset_0[2]) * _sensor_correction.gyro_scale_0[2];
} else if (_selected_gyro == 1) {
rates(0) = (_sensor_gyro.x - _sensor_correction.gyro_offset_1[0]) * _sensor_correction.gyro_scale_1[0];
rates(1) = (_sensor_gyro.y - _sensor_correction.gyro_offset_1[1]) * _sensor_correction.gyro_scale_1[1];
rates(2) = (_sensor_gyro.z - _sensor_correction.gyro_offset_1[2]) * _sensor_correction.gyro_scale_1[2];
} else if (_selected_gyro == 2) {
rates(0) = (_sensor_gyro.x - _sensor_correction.gyro_offset_2[0]) * _sensor_correction.gyro_scale_2[0];
rates(1) = (_sensor_gyro.y - _sensor_correction.gyro_offset_2[1]) * _sensor_correction.gyro_scale_2[1];
rates(2) = (_sensor_gyro.z - _sensor_correction.gyro_offset_2[2]) * _sensor_correction.gyro_scale_2[2];
} else {
rates(0) = _sensor_gyro.x;
rates(1) = _sensor_gyro.y;
rates(2) = _sensor_gyro.z;
}
// rotate corrected measurements from sensor to body frame
rates = _board_rotation * rates;
// correct for in-run bias errors
rates(0) -= _sensor_bias.gyro_x_bias;
rates(1) -= _sensor_bias.gyro_y_bias;
rates(2) -= _sensor_bias.gyro_z_bias;
math::Vector<3> rates_p_scaled = _params.rate_p.emult(pid_attenuations(_params.tpa_breakpoint_p, _params.tpa_rate_p));
//math::Vector<3> rates_i_scaled = _params.rate_i.emult(pid_attenuations(_params.tpa_breakpoint_i, _params.tpa_rate_i));
math::Vector<3> rates_d_scaled = _params.rate_d.emult(pid_attenuations(_params.tpa_breakpoint_d, _params.tpa_rate_d));
/* angular rates error */
math::Vector<3> rates_err = _rates_sp - rates;
_att_control = rates_p_scaled.emult(rates_err) +
_rates_int +
rates_d_scaled.emult(_rates_prev - rates) / dt +
_params.rate_ff.emult(_rates_sp);
_rates_sp_prev = _rates_sp;
_rates_prev = rates;
/* update integral only if motors are providing enough thrust to be effective */
if (_thrust_sp > MIN_TAKEOFF_THRUST) {
for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) {
// Check for positive control saturation
bool positive_saturation =
((i == AXIS_INDEX_ROLL) && _saturation_status.flags.roll_pos) ||
((i == AXIS_INDEX_PITCH) && _saturation_status.flags.pitch_pos) ||
((i == AXIS_INDEX_YAW) && _saturation_status.flags.yaw_pos);
// Check for negative control saturation
bool negative_saturation =
((i == AXIS_INDEX_ROLL) && _saturation_status.flags.roll_neg) ||
((i == AXIS_INDEX_PITCH) && _saturation_status.flags.pitch_neg) ||
((i == AXIS_INDEX_YAW) && _saturation_status.flags.yaw_neg);
// prevent further positive control saturation
if (positive_saturation) {
rates_err(i) = math::min(rates_err(i), 0.0f);
}
// prevent further negative control saturation
if (negative_saturation) {
rates_err(i) = math::max(rates_err(i), 0.0f);
}
// Perform the integration using a first order method and do not propaate the result if out of range or invalid
float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
if (PX4_ISFINITE(rate_i) && rate_i > -_params.rate_int_lim(i) && rate_i < _params.rate_int_lim(i)) {
_rates_int(i) = rate_i;
}
}
}
/* explicitly limit the integrator state */
for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) {
_rates_int(i) = math::constrain(_rates_int(i), -_params.rate_int_lim(i), _params.rate_int_lim(i));
}
}
void
MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[])
{
mc_att_control::g_control->task_main();
}
void
MulticopterAttitudeControl::task_main()
{
/*
* do subscriptions
*/
_v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits));
_battery_status_sub = orb_subscribe(ORB_ID(battery_status));
_gyro_count = math::min(orb_group_count(ORB_ID(sensor_gyro)), MAX_GYRO_COUNT);
if (_gyro_count == 0) {
_gyro_count = 1;
}
for (unsigned s = 0; s < _gyro_count; s++) {
_sensor_gyro_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
}
_sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));
_sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias));
/* initialize parameters cache */
parameters_update();
/* wakeup source: gyro data from sensor selected by the sensor app */
px4_pollfd_struct_t poll_fds = {};
poll_fds.events = POLLIN;
while (!_task_should_exit) {
poll_fds.fd = _sensor_gyro_sub[_selected_gyro];
/* wait for up to 100ms for data */
int pret = px4_poll(&poll_fds, 1, 100);
/* timed out - periodic check for _task_should_exit */
if (pret == 0) {
continue;
}
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
warn("mc att ctrl: poll error %d, %d", pret, errno);
/* sleep a bit before next try */
usleep(100000);
continue;
}
perf_begin(_loop_perf);
/* run controller on gyro changes */
if (poll_fds.revents & POLLIN) {
static uint64_t last_run = 0;
float dt = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
/* guard against too small (< 2ms) and too large (> 20ms) dt's */
if (dt < 0.002f) {
dt = 0.002f;
} else if (dt > 0.02f) {
dt = 0.02f;
}
/* copy gyro data */
orb_copy(ORB_ID(sensor_gyro), _sensor_gyro_sub[_selected_gyro], &_sensor_gyro);
/* check for updates in other topics */
parameter_update_poll();
vehicle_control_mode_poll();
arming_status_poll();
vehicle_manual_poll();
vehicle_status_poll();
vehicle_motor_limits_poll();
battery_status_poll();
vehicle_attitude_poll();
sensor_correction_poll();
sensor_bias_poll();
/* Check if we are in rattitude mode and the pilot is above the threshold on pitch
* or roll (yaw can rotate 360 in normal att control). If both are true don't
* even bother running the attitude controllers */
if (_v_control_mode.flag_control_rattitude_enabled) {
if (fabsf(_manual_control_sp.y) > _params.rattitude_thres ||
fabsf(_manual_control_sp.x) > _params.rattitude_thres) {
_v_control_mode.flag_control_attitude_enabled = false;
}
}
if (_v_control_mode.flag_control_attitude_enabled) {
if (_ts_opt_recovery == nullptr) {
// the tailsitter recovery instance has not been created, thus, the vehicle
// is not a tailsitter, do normal attitude control
control_attitude(dt);
} else {
vehicle_attitude_setpoint_poll();
_thrust_sp = _v_att_sp.thrust;
math::Quaternion q(_v_att.q[0], _v_att.q[1], _v_att.q[2], _v_att.q[3]);
math::Quaternion q_sp(&_v_att_sp.q_d[0]);
_ts_opt_recovery->setAttGains(_params.att_p, _params.yaw_ff);
_ts_opt_recovery->calcOptimalRates(q, q_sp, _v_att_sp.yaw_sp_move_rate, _rates_sp);
/* limit rates */
for (int i = 0; i < 3; i++) {
_rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i));
}
}
/* publish attitude rates setpoint */
_v_rates_sp.roll = _rates_sp(0);
_v_rates_sp.pitch = _rates_sp(1);
_v_rates_sp.yaw = _rates_sp(2);
_v_rates_sp.thrust = _thrust_sp;
_v_rates_sp.timestamp = hrt_absolute_time();
if (_v_rates_sp_pub != nullptr) {
orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
} else if (_rates_sp_id) {
_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
}
} else {
/* attitude controller disabled, poll rates setpoint topic */
if (_v_control_mode.flag_control_manual_enabled) {
/* manual rates control - ACRO mode */
_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x,
_manual_control_sp.r).emult(_params.acro_rate_max);
_thrust_sp = _manual_control_sp.z;
/* publish attitude rates setpoint */
_v_rates_sp.roll = _rates_sp(0);
_v_rates_sp.pitch = _rates_sp(1);
_v_rates_sp.yaw = _rates_sp(2);
_v_rates_sp.thrust = _thrust_sp;
_v_rates_sp.timestamp = hrt_absolute_time();
if (_v_rates_sp_pub != nullptr) {
orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
} else if (_rates_sp_id) {
_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
}
} else {
/* attitude controller disabled, poll rates setpoint topic */
vehicle_rates_setpoint_poll();
_rates_sp(0) = _v_rates_sp.roll;
_rates_sp(1) = _v_rates_sp.pitch;
_rates_sp(2) = _v_rates_sp.yaw;
_thrust_sp = _v_rates_sp.thrust;
}
}
if (_v_control_mode.flag_control_rates_enabled) {
control_attitude_rates(dt);
/* publish actuator controls */
_actuators.control[0] = (PX4_ISFINITE(_att_control(0))) ? _att_control(0) : 0.0f;
_actuators.control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f;
_actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f;
_actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f;
_actuators.control[7] = _v_att_sp.landing_gear;
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _v_att.timestamp;
/* scale effort by battery status */
if (_params.bat_scale_en && _battery_status.scale > 0.0f) {
for (int i = 0; i < 4; i++) {
_actuators.control[i] *= _battery_status.scale;
}
}
_controller_status.roll_rate_integ = _rates_int(0);
_controller_status.pitch_rate_integ = _rates_int(1);
_controller_status.yaw_rate_integ = _rates_int(2);
_controller_status.timestamp = hrt_absolute_time();
if (!_actuators_0_circuit_breaker_enabled) {
if (_actuators_0_pub != nullptr) {
orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
perf_end(_controller_latency_perf);
} else if (_actuators_id) {
_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
}
}
/* publish controller status */
if (_controller_status_pub != nullptr) {
orb_publish(ORB_ID(mc_att_ctrl_status), _controller_status_pub, &_controller_status);
} else {
_controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status);
}
}
if (_v_control_mode.flag_control_termination_enabled) {
if (!_vehicle_status.is_vtol) {
_rates_sp.zero();
_rates_int.zero();
_thrust_sp = 0.0f;
_att_control.zero();
/* publish actuator controls */
_actuators.control[0] = 0.0f;
_actuators.control[1] = 0.0f;
_actuators.control[2] = 0.0f;
_actuators.control[3] = 0.0f;
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _v_att.timestamp;
if (!_actuators_0_circuit_breaker_enabled) {
if (_actuators_0_pub != nullptr) {
orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
perf_end(_controller_latency_perf);
} else if (_actuators_id) {
_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
}
}
_controller_status.roll_rate_integ = _rates_int(0);
_controller_status.pitch_rate_integ = _rates_int(1);
_controller_status.yaw_rate_integ = _rates_int(2);
_controller_status.timestamp = hrt_absolute_time();
/* publish controller status */
if (_controller_status_pub != nullptr) {
orb_publish(ORB_ID(mc_att_ctrl_status), _controller_status_pub, &_controller_status);
} else {
_controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status);
}
/* publish attitude rates setpoint */
_v_rates_sp.roll = _rates_sp(0);
_v_rates_sp.pitch = _rates_sp(1);
_v_rates_sp.yaw = _rates_sp(2);
_v_rates_sp.thrust = _thrust_sp;
_v_rates_sp.timestamp = hrt_absolute_time();
if (_v_rates_sp_pub != nullptr) {
orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
} else if (_rates_sp_id) {
_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
}
}
}
}
perf_end(_loop_perf);
}
_control_task = -1;
}
int
MulticopterAttitudeControl::start()
{
ASSERT(_control_task == -1);
/* start the task */
_control_task = px4_task_spawn_cmd("mc_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_ATTITUDE_CONTROL,
1700,
(px4_main_t)&MulticopterAttitudeControl::task_main_trampoline,
nullptr);
if (_control_task < 0) {
warn("task start failed");
return -errno;
}
return OK;
}
int mc_att_control_main(int argc, char *argv[])
{
if (argc < 2) {
warnx("usage: mc_att_control {start|stop|status}");
return 1;
}
if (!strcmp(argv[1], "start")) {
if (mc_att_control::g_control != nullptr) {
warnx("already running");
return 1;
}
mc_att_control::g_control = new MulticopterAttitudeControl;
if (mc_att_control::g_control == nullptr) {
warnx("alloc failed");
return 1;
}
if (OK != mc_att_control::g_control->start()) {
delete mc_att_control::g_control;
mc_att_control::g_control = nullptr;
warnx("start failed");
return 1;
}
return 0;
}
if (!strcmp(argv[1], "stop")) {
if (mc_att_control::g_control == nullptr) {
warnx("not running");
return 1;
}
delete mc_att_control::g_control;
mc_att_control::g_control = nullptr;
return 0;
}
if (!strcmp(argv[1], "status")) {
if (mc_att_control::g_control) {
warnx("running");
return 0;
} else {
warnx("not running");
return 1;
}
}
warnx("unrecognized command");
return 1;
}
Reference in New Issue View Git Blame Copy Permalink